| 47,7 → 47,8 |
|---|
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 56,7 → 57,7 |
|---|
| #include "eeprom.c" |
| #include "mymath.h" |
| |
| unsigned char h,m,s; |
| unsigned char h,m,s,BaroExpandActive = 0; |
| volatile unsigned int I2CTimeout = 100; |
| int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
| int TrimNick, TrimRoll; |
| 111,6 → 112,8 |
|---|
| unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
| unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
| unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
| unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250 |
| unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-250 |
| unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
| unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
| unsigned char Parameter_UserParam1 = 0; |
| 284,11 → 287,6 |
|---|
| MesswertRoll = (signed int) AdWertRollFilter / 8; |
| RohMesswertNick = MesswertNick; |
| RohMesswertRoll = MesswertRoll; |
| //DebugOut.Analog[21] = MesswertNick; |
| //DebugOut.Analog[22] = MesswertRoll; |
| //DebugOut.Analog[22] = Mess_Integral_Gier; |
| //DebugOut.Analog[21] = MesswertNick; |
| //DebugOut.Analog[22] = MesswertRoll; |
| |
| // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
| Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; |
| 510,6 → 508,8 |
|---|
| CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
| CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
| CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
| CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255); |
| CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I,0,255); |
| CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
| CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
| CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
| 536,7 → 536,6 |
|---|
| } |
| |
| |
| |
| //############################################################################ |
| // |
| void MotorRegler(void) |
| 750,8 → 749,8 |
|---|
| |
| GyroFaktor = (Parameter_Gyro_P + 10.0); |
| IntegralFaktor = Parameter_Gyro_I; |
| GyroFaktorGier = (Parameter_Gyro_P + 10.0); |
| IntegralFaktorGier = Parameter_Gyro_I; |
| GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
| IntegralFaktorGier = Parameter_Gyro_Gier_I; |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //+ Analoge Steuerung per Seriell |
| 1192,42 → 1191,17 |
|---|
| DebugOut.Analog[18] = VarioMeter; |
| DebugOut.Analog[19] = WinkelOut.CalcState; |
| DebugOut.Analog[20] = ServoValue; |
| DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
| // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
| // DebugOut.Analog[24] = MesswertNick/2; |
| // DebugOut.Analog[25] = MesswertRoll/2; |
| DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
| // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
| // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
| // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
| //DebugOut.Analog[28] = I2CError; |
| DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
| DebugOut.Analog[30] = GPS_Nick; |
| DebugOut.Analog[31] = GPS_Roll; |
| |
| |
| // DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
| // if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
| |
| /* 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] = MesswertNick; |
| // DebugOut.Analog[9] = SollHoehe; |
| // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
| // DebugOut.Analog[11] = KompassStartwert; |
| // DebugOut.Analog[10] = Parameter_Gyro_I; |
| // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
| // DebugOut.Analog[9] = KompassRichtung; |
| // DebugOut.Analog[10] = GasMischanteil; |
| // DebugOut.Analog[3] = HoeheD * 32; |
| // DebugOut.Analog[4] = hoehenregler; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1263,8 → 1237,9 |
|---|
| if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
| GasMischanteil *= STICK_GAIN; |
| |
| DebugOut.Analog[16] = BaroExpandActive; |
| // if height control is activated |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
| { |
| #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
| 1272,14 → 1247,14 |
|---|
| int HCGas, HeightDeviation; |
| static int HeightTrimming = 0; // rate for change of height setpoint |
| static int FilterHCGas = 0; |
| static int StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static unsigned long HooverGasFilter = 0; |
| static unsigned char delay = 100; |
| static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| int CosAttitude; // for projection of hoover gas |
| long tmp_long3; |
| // const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
| // get the current hooverpoint |
| DebugOut.Analog[25] = HooverGas; |
| DebugOut.Analog[21] = HooverGas; |
| DebugOut.Analog[18] = VarioMeter; |
| |
| // if height control is activated by an rc channel |
| 1289,28 → 1264,9 |
|---|
| { //height control not active |
| if(!delay--) |
| { |
| // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
| if( (MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
| { // increase offset |
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| beeptime = 300; |
| delay = 250; |
| } |
| // measurement of air pressure close to lower limit and |
| else if( (MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP) ) |
| { // decrease offset |
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| beeptime = 300; |
| delay = 250; |
| } |
| else |
| { |
| SollHoehe = HoehenWert; // update SetPoint with current reading |
| HoehenReglerAktiv = 0; // disable height control |
| delay = 1; |
| } |
| HoehenReglerAktiv = 0; // disable height control |
| SollHoehe = HoehenWert; // update SetPoint with current reading |
| delay = 1; |
| } |
| } |
| else |
| 1318,6 → 1274,55 |
|---|
| HoehenReglerAktiv = 1; // enable height control |
| delay = 200; |
| } |
| // Expand the measurement |
| // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
| if(!BaroExpandActive) |
| { |
| if(MessLuftdruck > 920) |
| { // increase offset |
| if(OCR0A < (255 - OPA_OFFSET_STEP)) |
| { |
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| beeptime = 300; |
| delay = 250; |
| BaroExpandActive = 250; |
| } |
| else |
| { |
| BaroAtLowerLimit = 1; |
| } |
| } |
| // measurement of air pressure close to lower limit and |
| else |
| if(MessLuftdruck < 100) |
| { // decrease offset |
| if(OCR0A > OPA_OFFSET_STEP) |
| { |
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| beeptime = 300; |
| delay = 250; |
| BaroExpandActive = 250; |
| } |
| else |
| { |
| BaroAtUpperLimit = 1; |
| } |
| } |
| else |
| { |
| BaroAtUpperLimit = 0; |
| BaroAtLowerLimit = 0; |
| } |
| } |
| else // delay, because of expanding the Baro-Range |
| { |
| // now clear the D-values |
| SummenHoehe = HoehenWert * SM_FILTER; |
| VarioMeter = 0; |
| BaroExpandActive--; |
| } |
| } |
| else // no switchable height control |
| { |
| 1324,6 → 1329,7 |
|---|
| SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
| HoehenReglerAktiv = 1; |
| } |
| |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
| tmp_long *= tmp_long; |
| 1359,7 → 1365,7 |
|---|
| // the setpoint will be fine adjusted with the gas stick position |
| if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
| { // gas stick is above hoover point |
| if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
| { |
| 1369,7 → 1375,7 |
|---|
| HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
| HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
| } // gas stick is below hoover point |
| else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
| { |
| 1396,16 → 1402,16 |
|---|
| HeightTrimming = 0; |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| //update hoover gas stick value when setpoint is shifted |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| if(StickGasHoover < 70) StickGasHoover = 70; else |
| if(StickGasHoover > 150) StickGasHoover = 150; |
| // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| // if(StickGasHoover < 70) StickGasHoover = 70; |
| // else if(StickGasHoover > 150) StickGasHoover = 150; |
| } |
| } //if MikroKopterFlags & MKFLAG_FLY |
| else |
| { |
| SollHoehe = HoehenWert - 200; |
| StickGasHoover = 110; |
| SollHoehe = HoehenWert - 400; |
| StickGasHoover = 120; |
| } |
| HCGas = HooverGas; // take hoover gas (neutral point) |
| } |
| 1426,7 → 1432,6 |
|---|
| HCGas -= tmp_int; |
| // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| DebugOut.Analog[23] = -tmp_int; |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| |
| 1436,23 → 1441,18 |
|---|
| LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| } |
| |
| if(BaroExpandActive) HCGas = HooverGas; |
| // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
| DebugOut.Analog[24] = -tmp_int; |
| LIMIT_MIN_MAX(tmp_int, -127, 255); |
| HCGas -= tmp_int; |
| // strech control output by inverse attitude projection |
| // + 1/cos(angle) ++++++++++++++++++++++++++ |
| // strech control output by inverse attitude projection 1/cos |
| |
| // strech control output by inverse attitude projection 1/cos |
| // + 1/cos(angle) ++++++++++++++++++++++++++ |
| tmp_long3 = (int32_t)HCGas; |
| tmp_long3 *= 8192L; |
| tmp_long3 /= CosAttitude; |
| HCGas = (int16_t)tmp_long3; |
| /* |
| tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
| tmp_long3 /= 1500;//1024 * 2; |
| if(tmp_long3 > 29) tmp_long3 = 29; |
| HCGas = ((long) HCGas * GAS_TAB[tmp_long3]) / 128L; |
| */ |
| // update height control gas averaging |
| FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
| // limit height control gas pd-control output |
| 1468,8 → 1468,14 |
|---|
| else // HC not active |
| { |
| //update hoover gas stick value when HC is not active |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| if(!EE_Parameter.Hoehe_StickNeutralPoint) |
| { |
| StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| } |
| else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| if(StickGasHoover < 70) StickGasHoover = 70; |
| else if(StickGasHoover > 150) StickGasHoover = 150; |
| } |
| |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| 1479,12 → 1485,6 |
|---|
| if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
| if(abs(VarioMeter) < 100) // only on small vertical speed |
| { |
| /* tmp_long3 = (long)GasMischanteil; // take current thrust |
| tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
| tmp_long3 /= 1500;//1024 * 2; |
| if(tmp_long3 > 29) tmp_long3 = 29; |
| tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3]; |
| */ |
| tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
| tmp_long3 *= CosAttitude; // apply attitude projection |
| tmp_long3 /= 8192; |
| 1526,9 → 1526,7 |
|---|
| } |
| } |
| DebugOut.Analog[25] = HooverGas; |
| DebugOut.Analog[26] = HooverGasMax; |
| DebugOut.Analog[28] = HooverGasMin; |
| |
| //DebugOut.Analog[26] = HooverGasMax; |
| }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| |
| // limit gas to parameter setting |
| 1551,9 → 1549,7 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Gier-Anteil |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define MUL_G 1.0 |
| GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
| // GierMischanteil = 0; |
| #define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
| if(GasMischanteil > MIN_GIERGAS) |
| { |