| 91,7 → 91,7 |
|---|
| volatile int32_t IntegralYaw = 0; |
| volatile int32_t Reading_IntegralGyroPitch = 0, Reading_IntegralGyroPitch2 = 0; |
| volatile int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0; |
| volatile int32_t Reading_IntegralGyroYaw = 0, Reading_IntegralGyroYaw2 = 0; |
| volatile int32_t Reading_IntegralGyroYaw = 0; |
| volatile int32_t MeanIntegralPitch; |
| volatile int32_t MeanIntegralRoll; |
| |
| 242,7 → 242,6 |
|---|
| // Yaw |
| // calculate yaw gyro intergral (~ to rotation angle) |
| Reading_IntegralGyroYaw += Reading_GyroYaw; |
| Reading_IntegralGyroYaw2 += Reading_GyroYaw; |
| // Coupling fraction |
| if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
| { |
| 436,7 → 435,7 |
|---|
| static uint8_t HeightControlActive = 0; |
| static int16_t HeightControlThrust = 0; |
| static int8_t TimerDebugOut = 0; |
| static int8_t StoreNewCompassCourse = 0; |
| static uint16_t UpdateCompassCourse = 0; |
| static int32_t CorrectionPitch, CorrectionRoll; |
| |
| Mean(); |
| 504,7 → 503,6 |
|---|
| SumPitch = 0; |
| SumRoll = 0; |
| Reading_IntegralGyroYaw = 0; |
| Reading_IntegralGyroYaw2 = 0; |
| } |
| |
| if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
| 576,18 → 574,18 |
|---|
| // save the ACC neutral setting to eeprom |
| else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
| { |
| if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
| if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
| { |
| delay_neutral = 0; |
| GRN_OFF; |
| SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid |
| Model_Is_Flying = 0; |
| SetNeutral(); |
| // Save ACC neutral settings to eeprom |
| SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
| SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
| SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
| Beep(GetActiveParamSet()); |
| delay_neutral = 0; |
| GRN_OFF; |
| SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid |
| Model_Is_Flying = 0; |
| SetNeutral(); |
| // Save ACC neutral settings to eeprom |
| SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
| SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
| SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
| Beep(GetActiveParamSet()); |
| } |
| } |
| else delay_neutral = 0; |
| 609,7 → 607,6 |
|---|
| MotorsOn = 1; |
| SetPointYaw = 0; |
| Reading_IntegralGyroYaw = 0; |
| Reading_IntegralGyroYaw2 = 0; |
| Reading_IntegralGyroPitch = 0; |
| Reading_IntegralGyroRoll = 0; |
| Reading_IntegralGyroPitch2 = IntegralPitch; |
| 1011,15 → 1008,14 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Yawing |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 20) // yaw stick is activated |
| { // if not fixed compass course is set update compass course |
| if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1; |
| if(abs(StickYaw) > 20 ) // yaw stick is activated |
| { // if not fixed compass course is set update compass course for 1s |
| if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) UpdateCompassCourse = 500; |
| } |
| // 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; |
| 1043,7 → 1039,6 |
|---|
| #ifdef USE_CMPS03 |
| CompassHeading = CMPS03_Heading(); |
| #endif |
| |
| if (CompassHeading < 0) // no compass data available |
| { |
| CompassOffCourse = 0; // disables gyro compass correction |
| 1051,17 → 1046,17 |
|---|
| 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 then update compass course if neccessary |
| if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) // 35 corresponds to a declination of ~14 deg |
| if((w < 70) && UpdateCompassCourse && (CompassHeading>=0) ) // 70 corresponds to a declination of ~28 deg |
| { |
| CompassCourse = CompassHeading; |
| StoreNewCompassCourse = 0; |
| CompassOffCourse = 0; // should be set immediatly because the CompassOffCourse is otherwise only refreshed if new compass heading is calulated |
| } |
| if(UpdateCompassCourse) UpdateCompassCourse--; |
| w = (w * FCParam.CompassYawEffect) / 64; // (w=0 for 64->~25 deg, 128->~50 deg) for higher declinaions the compass drift compensation is disabled |
| w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination |
| if(w > 0) // if there is any compass effect (avoid negative compass feedback) |