Rev 1320 | Rev 1328 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1320 | Rev 1322 | ||
---|---|---|---|
Line 45... | Line 45... | ||
45 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
45 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
46 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
46 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
47 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
47 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
48 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
48 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
49 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
49 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
- | 50 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
|
50 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
51 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
51 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
52 | // + POSSIBILITY OF SUCH DAMAGE. |
53 | // + POSSIBILITY OF SUCH DAMAGE. |
53 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 54... | Line 55... | ||
54 | 55 | ||
55 | #include "main.h" |
56 | #include "main.h" |
56 | #include "eeprom.c" |
57 | #include "eeprom.c" |
Line 57... | Line 58... | ||
57 | #include "mymath.h" |
58 | #include "mymath.h" |
58 | 59 | ||
59 | unsigned char h,m,s; |
60 | unsigned char h,m,s,BaroExpandActive = 0; |
60 | volatile unsigned int I2CTimeout = 100; |
61 | volatile unsigned int I2CTimeout = 100; |
61 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
62 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
62 | int TrimNick, TrimRoll; |
63 | int TrimNick, TrimRoll; |
Line 109... | Line 110... | ||
109 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
110 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
110 | unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250 |
111 | unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250 |
111 | unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
112 | unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
112 | unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
113 | unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
113 | unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
114 | unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
- | 115 | unsigned char Parameter_Gyro_Gier_P = 150; // Wert : 10-250 |
|
- | 116 | unsigned char Parameter_Gyro_Gier_I = 150; // Wert : 10-250 |
|
114 | unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
117 | unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
115 | unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
118 | unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
116 | unsigned char Parameter_UserParam1 = 0; |
119 | unsigned char Parameter_UserParam1 = 0; |
117 | unsigned char Parameter_UserParam2 = 0; |
120 | unsigned char Parameter_UserParam2 = 0; |
118 | unsigned char Parameter_UserParam3 = 0; |
121 | unsigned char Parameter_UserParam3 = 0; |
Line 282... | Line 285... | ||
282 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
285 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
283 | MesswertNick = (signed int) AdWertNickFilter / 8; |
286 | MesswertNick = (signed int) AdWertNickFilter / 8; |
284 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
287 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
285 | RohMesswertNick = MesswertNick; |
288 | RohMesswertNick = MesswertNick; |
286 | RohMesswertRoll = MesswertRoll; |
289 | RohMesswertRoll = MesswertRoll; |
287 | //DebugOut.Analog[21] = MesswertNick; |
- | |
288 | //DebugOut.Analog[22] = MesswertRoll; |
- | |
289 | //DebugOut.Analog[22] = Mess_Integral_Gier; |
- | |
290 | //DebugOut.Analog[21] = MesswertNick; |
- | |
291 | //DebugOut.Analog[22] = MesswertRoll; |
- | |
Line 292... | Line 290... | ||
292 | 290 | ||
293 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
291 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
294 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; |
292 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; |
295 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L; |
293 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L; |
Line 508... | Line 506... | ||
508 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,0,255); |
506 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,0,255); |
509 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
507 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
510 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
508 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
511 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
509 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
512 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
510 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
- | 511 | CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255); |
|
- | 512 | CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I,0,255); |
|
513 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
513 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
514 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
514 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
515 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
515 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
516 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
516 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
517 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
517 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
Line 534... | Line 534... | ||
534 | MAX_GAS = EE_Parameter.Gas_Max; |
534 | MAX_GAS = EE_Parameter.Gas_Max; |
535 | MIN_GAS = EE_Parameter.Gas_Min; |
535 | MIN_GAS = EE_Parameter.Gas_Min; |
536 | } |
536 | } |
Line 537... | Line -... | ||
537 | - | ||
538 | 537 | ||
539 | 538 | ||
540 | //############################################################################ |
539 | //############################################################################ |
541 | // |
540 | // |
542 | void MotorRegler(void) |
541 | void MotorRegler(void) |
Line 748... | Line 747... | ||
748 | 747 | ||
Line 749... | Line 748... | ||
749 | StickGas = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2; |
748 | StickGas = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2; |
750 | 749 | ||
751 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
750 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
752 | IntegralFaktor = Parameter_Gyro_I; |
751 | IntegralFaktor = Parameter_Gyro_I; |
Line 753... | Line 752... | ||
753 | GyroFaktorGier = (Parameter_Gyro_P + 10.0); |
752 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
754 | IntegralFaktorGier = Parameter_Gyro_I; |
753 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
755 | 754 | ||
756 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
755 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1190... | Line 1189... | ||
1190 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1189 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1191 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1190 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1192 | DebugOut.Analog[18] = VarioMeter; |
1191 | DebugOut.Analog[18] = VarioMeter; |
1193 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1192 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1194 | DebugOut.Analog[20] = ServoValue; |
1193 | DebugOut.Analog[20] = ServoValue; |
1195 | DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
1194 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
1196 | // DebugOut.Analog[24] = MesswertNick/2; |
1195 | // DebugOut.Analog[24] = MesswertNick/2; |
1197 | // DebugOut.Analog[25] = MesswertRoll/2; |
1196 | // DebugOut.Analog[25] = MesswertRoll/2; |
1198 | DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1197 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1199 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1198 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1200 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1199 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
- | 1200 | //DebugOut.Analog[28] = I2CError; |
|
1201 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
1201 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
1202 | DebugOut.Analog[30] = GPS_Nick; |
1202 | DebugOut.Analog[30] = GPS_Nick; |
1203 | DebugOut.Analog[31] = GPS_Roll; |
1203 | DebugOut.Analog[31] = GPS_Roll; |
Line 1204... | Line -... | ||
1204 | - | ||
1205 | - | ||
1206 | // DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
- | |
1207 | // if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
- | |
1208 | - | ||
1209 | /* DebugOut.Analog[16] = motor_rx[0]; |
- | |
1210 | DebugOut.Analog[17] = motor_rx[1]; |
- | |
1211 | DebugOut.Analog[18] = motor_rx[2]; |
- | |
1212 | DebugOut.Analog[19] = motor_rx[3]; |
- | |
1213 | DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
- | |
1214 | DebugOut.Analog[20] /= 14; |
- | |
1215 | DebugOut.Analog[21] = motor_rx[4]; |
- | |
1216 | DebugOut.Analog[22] = motor_rx[5]; |
- | |
1217 | DebugOut.Analog[23] = motor_rx[6]; |
- | |
1218 | DebugOut.Analog[24] = motor_rx[7]; |
- | |
1219 | DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
- | |
1220 | */ |
- | |
1221 | // DebugOut.Analog[9] = MesswertNick; |
- | |
1222 | // DebugOut.Analog[9] = SollHoehe; |
- | |
1223 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
- | |
1224 | // DebugOut.Analog[11] = KompassStartwert; |
- | |
1225 | // DebugOut.Analog[10] = Parameter_Gyro_I; |
- | |
1226 | // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
- | |
1227 | // DebugOut.Analog[9] = KompassRichtung; |
- | |
1228 | // DebugOut.Analog[10] = GasMischanteil; |
- | |
1229 | // DebugOut.Analog[3] = HoeheD * 32; |
- | |
1230 | // DebugOut.Analog[4] = hoehenregler; |
1204 | |
Line 1231... | Line 1205... | ||
1231 | } |
1205 | } |
1232 | 1206 | ||
1233 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1207 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1261... | Line 1235... | ||
1261 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1235 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1262 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1236 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1263 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1237 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1264 | GasMischanteil *= STICK_GAIN; |
1238 | GasMischanteil *= STICK_GAIN; |
Line -... | Line 1239... | ||
- | 1239 | ||
1265 | 1240 | DebugOut.Analog[16] = BaroExpandActive; |
|
1266 | // if height control is activated |
1241 | // if height control is activated |
1267 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1242 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1268 | { |
1243 | { |
1269 | #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1244 | #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1270 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
1245 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
1271 | #define OPA_OFFSET_STEP 10 |
1246 | #define OPA_OFFSET_STEP 10 |
1272 | int HCGas, HeightDeviation; |
1247 | int HCGas, HeightDeviation; |
1273 | static int HeightTrimming = 0; // rate for change of height setpoint |
1248 | static int HeightTrimming = 0; // rate for change of height setpoint |
1274 | static int FilterHCGas = 0; |
1249 | static int FilterHCGas = 0; |
1275 | static int StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
1250 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
1276 | static unsigned long HooverGasFilter = 0; |
1251 | static unsigned long HooverGasFilter = 0; |
1277 | static unsigned char delay = 100; |
1252 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1278 | int CosAttitude; // for projection of hoover gas |
1253 | int CosAttitude; // for projection of hoover gas |
1279 | long tmp_long3; |
1254 | long tmp_long3; |
1280 | // 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}; |
1255 | // 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}; |
1281 | // get the current hooverpoint |
1256 | // get the current hooverpoint |
1282 | DebugOut.Analog[25] = HooverGas; |
1257 | DebugOut.Analog[21] = HooverGas; |
Line 1283... | Line 1258... | ||
1283 | DebugOut.Analog[18] = VarioMeter; |
1258 | DebugOut.Analog[18] = VarioMeter; |
1284 | 1259 | ||
1285 | // if height control is activated by an rc channel |
1260 | // if height control is activated by an rc channel |
1286 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1261 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1287 | { // check if parameter is less than activation threshold |
1262 | { // check if parameter is less than activation threshold |
1288 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1263 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1289 | { //height control not active |
1264 | { //height control not active |
1290 | if(!delay--) |
- | |
1291 | { |
- | |
1292 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
- | |
1293 | if( (MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) ) |
- | |
1294 | { // increase offset |
- | |
1295 | ExpandBaro -= 1; |
- | |
1296 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
- | |
1297 | beeptime = 300; |
- | |
1298 | delay = 250; |
1265 | if(!delay--) |
1299 | } |
- | |
1300 | // measurement of air pressure close to lower limit and |
- | |
1301 | else if( (MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP) ) |
- | |
1302 | { // decrease offset |
- | |
1303 | ExpandBaro += 1; |
- | |
1304 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
- | |
1305 | beeptime = 300; |
- | |
1306 | delay = 250; |
- | |
1307 | } |
- | |
1308 | else |
1266 | { |
1309 | { |
- | |
1310 | SollHoehe = HoehenWert; // update SetPoint with current reading |
1267 | HoehenReglerAktiv = 0; // disable height control |
1311 | HoehenReglerAktiv = 0; // disable height control |
- | |
1312 | delay = 1; |
1268 | SollHoehe = HoehenWert; // update SetPoint with current reading |
1313 | } |
1269 | delay = 1; |
1314 | } |
1270 | } |
1315 | } |
1271 | } |
1316 | else |
1272 | else |
1317 | { //height control is activated |
1273 | { //height control is activated |
1318 | HoehenReglerAktiv = 1; // enable height control |
1274 | HoehenReglerAktiv = 1; // enable height control |
- | 1275 | delay = 200; |
|
- | 1276 | } |
|
- | 1277 | // Expand the measurement |
|
- | 1278 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
|
- | 1279 | if(!BaroExpandActive) |
|
- | 1280 | { |
|
- | 1281 | if(MessLuftdruck > 920) |
|
- | 1282 | { // increase offset |
|
- | 1283 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
|
- | 1284 | { |
|
- | 1285 | ExpandBaro -= 1; |
|
- | 1286 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
|
- | 1287 | beeptime = 300; |
|
- | 1288 | delay = 250; |
|
- | 1289 | BaroExpandActive = 250; |
|
- | 1290 | } |
|
- | 1291 | else |
|
- | 1292 | { |
|
- | 1293 | BaroAtLowerLimit = 1; |
|
- | 1294 | } |
|
- | 1295 | } |
|
- | 1296 | // measurement of air pressure close to lower limit and |
|
- | 1297 | else |
|
- | 1298 | if(MessLuftdruck < 100) |
|
- | 1299 | { // decrease offset |
|
- | 1300 | if(OCR0A > OPA_OFFSET_STEP) |
|
- | 1301 | { |
|
- | 1302 | ExpandBaro += 1; |
|
- | 1303 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
|
- | 1304 | beeptime = 300; |
|
- | 1305 | delay = 250; |
|
- | 1306 | BaroExpandActive = 250; |
|
- | 1307 | } |
|
- | 1308 | else |
|
- | 1309 | { |
|
- | 1310 | BaroAtUpperLimit = 1; |
|
- | 1311 | } |
|
- | 1312 | } |
|
- | 1313 | else |
|
- | 1314 | { |
|
- | 1315 | BaroAtUpperLimit = 0; |
|
- | 1316 | BaroAtLowerLimit = 0; |
|
- | 1317 | } |
|
- | 1318 | } |
|
- | 1319 | else // delay, because of expanding the Baro-Range |
|
- | 1320 | { |
|
- | 1321 | // now clear the D-values |
|
- | 1322 | SummenHoehe = HoehenWert * SM_FILTER; |
|
- | 1323 | VarioMeter = 0; |
|
1319 | delay = 200; |
1324 | BaroExpandActive--; |
1320 | } |
1325 | } |
1321 | } |
1326 | } |
1322 | else // no switchable height control |
1327 | else // no switchable height control |
1323 | { |
1328 | { |
1324 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1329 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
- | 1330 | HoehenReglerAktiv = 1; |
|
1325 | HoehenReglerAktiv = 1; |
1331 | } |
1326 | } |
1332 | |
1327 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1333 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1328 | tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
1334 | tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
1329 | tmp_long *= tmp_long; |
1335 | tmp_long *= tmp_long; |
Line 1357... | Line 1363... | ||
1357 | // PD-Control with respect to hoover point |
1363 | // PD-Control with respect to hoover point |
1358 | // the thrust loss out of horizontal attitude is compensated |
1364 | // the thrust loss out of horizontal attitude is compensated |
1359 | // the setpoint will be fine adjusted with the gas stick position |
1365 | // the setpoint will be fine adjusted with the gas stick position |
1360 | if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
1366 | if(MikroKopterFlags & FLAG_FLY) // trim setpoint only when flying |
1361 | { // gas stick is above hoover point |
1367 | { // gas stick is above hoover point |
1362 | if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
1368 | if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1363 | { |
1369 | { |
1364 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1370 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1365 | { |
1371 | { |
1366 | HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
1372 | HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
1367 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1373 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1368 | } |
1374 | } |
1369 | HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
1375 | HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
1370 | HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1376 | HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1371 | } // gas stick is below hoover point |
1377 | } // gas stick is below hoover point |
1372 | else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
1378 | else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1373 | { |
1379 | { |
1374 | if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
1380 | if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
1375 | { |
1381 | { |
1376 | HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
1382 | HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
1377 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1383 | SollHoehe = HoehenWert; // update setpoint to current heigth |
Line 1394... | Line 1400... | ||
1394 | { |
1400 | { |
1395 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1401 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1396 | HeightTrimming = 0; |
1402 | HeightTrimming = 0; |
1397 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1403 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1398 | //update hoover gas stick value when setpoint is shifted |
1404 | //update hoover gas stick value when setpoint is shifted |
1399 | StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1405 | // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1400 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1406 | // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1401 | if(StickGasHoover < 70) StickGasHoover = 70; else |
1407 | // if(StickGasHoover < 70) StickGasHoover = 70; |
1402 | if(StickGasHoover > 150) StickGasHoover = 150; |
1408 | // else if(StickGasHoover > 150) StickGasHoover = 150; |
1403 | } |
1409 | } |
1404 | } //if MikroKopterFlags & MKFLAG_FLY |
1410 | } //if MikroKopterFlags & MKFLAG_FLY |
1405 | else |
1411 | else |
1406 | { |
1412 | { |
1407 | SollHoehe = HoehenWert - 200; |
1413 | SollHoehe = HoehenWert - 400; |
1408 | StickGasHoover = 110; |
1414 | StickGasHoover = 120; |
1409 | } |
1415 | } |
1410 | HCGas = HooverGas; // take hoover gas (neutral point) |
1416 | HCGas = HooverGas; // take hoover gas (neutral point) |
1411 | } |
1417 | } |
Line 1412... | Line 1418... | ||
1412 | 1418 | ||
Line 1424... | Line 1430... | ||
1424 | tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter |
1430 | tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter |
1425 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1431 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1426 | HCGas -= tmp_int; |
1432 | HCGas -= tmp_int; |
1427 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1433 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1428 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1434 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1429 | DebugOut.Analog[23] = -tmp_int; |
- | |
1430 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1435 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1431 | HCGas -= tmp_int; |
1436 | HCGas -= tmp_int; |
Line 1432... | Line 1437... | ||
1432 | 1437 | ||
1433 | // limit deviation from hoover point within the target region |
1438 | // limit deviation from hoover point within the target region |
1434 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1439 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1435 | { |
1440 | { |
1436 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1441 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
Line -... | Line 1442... | ||
- | 1442 | } |
|
1437 | } |
1443 | |
1438 | 1444 | if(BaroExpandActive) HCGas = HooverGas; |
|
1439 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1445 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1440 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
1446 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
- | 1447 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
|
1441 | DebugOut.Analog[24] = -tmp_int; |
1448 | HCGas -= tmp_int; |
1442 | HCGas -= tmp_int; |
1449 | |
1443 | // strech control output by inverse attitude projection |
- | |
1444 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1450 | // strech control output by inverse attitude projection 1/cos |
1445 | // strech control output by inverse attitude projection 1/cos |
1451 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1446 | tmp_long3 = (int32_t)HCGas; |
1452 | tmp_long3 = (int32_t)HCGas; |
1447 | tmp_long3 *= 8192L; |
1453 | tmp_long3 *= 8192L; |
1448 | tmp_long3 /= CosAttitude; |
- | |
1449 | HCGas = (int16_t)tmp_long3; |
- | |
1450 | /* |
- | |
1451 | tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
- | |
1452 | tmp_long3 /= 1500;//1024 * 2; |
- | |
1453 | if(tmp_long3 > 29) tmp_long3 = 29; |
- | |
1454 | HCGas = ((long) HCGas * GAS_TAB[tmp_long3]) / 128L; |
1454 | tmp_long3 /= CosAttitude; |
1455 | */ |
1455 | HCGas = (int16_t)tmp_long3; |
1456 | // update height control gas averaging |
1456 | // update height control gas averaging |
1457 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1457 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1458 | // limit height control gas pd-control output |
1458 | // limit height control gas pd-control output |
Line 1466... | Line 1466... | ||
1466 | } |
1466 | } |
1467 | }// EOF height control active |
1467 | }// EOF height control active |
1468 | else // HC not active |
1468 | else // HC not active |
1469 | { |
1469 | { |
1470 | //update hoover gas stick value when HC is not active |
1470 | //update hoover gas stick value when HC is not active |
- | 1471 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
|
- | 1472 | { |
|
1471 | StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1473 | StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1472 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1474 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
- | 1475 | } |
|
- | 1476 | else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
|
- | 1477 | if(StickGasHoover < 70) StickGasHoover = 70; |
|
- | 1478 | else if(StickGasHoover > 150) StickGasHoover = 150; |
|
1473 | } |
1479 | } |
Line 1474... | Line 1480... | ||
1474 | 1480 | ||
1475 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1481 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1476 | // this is done only if height contol option is selected in global config and aircraft is flying |
1482 | // this is done only if height contol option is selected in global config and aircraft is flying |
1477 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1483 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1478 | { |
1484 | { |
1479 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1485 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1480 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1486 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1481 | { |
- | |
1482 | /* tmp_long3 = (long)GasMischanteil; // take current thrust |
- | |
1483 | tmp_long3 = labs(IntegralNick) + labs(IntegralRoll); |
- | |
1484 | tmp_long3 /= 1500;//1024 * 2; |
- | |
1485 | if(tmp_long3 > 29) tmp_long3 = 29; |
- | |
1486 | tmp_long3 = ((long) GasMischanteil * 128L) / (long) GAS_TAB[tmp_long3]; |
- | |
1487 | */ |
1487 | { |
1488 | tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
1488 | tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
1489 | tmp_long3 *= CosAttitude; // apply attitude projection |
1489 | tmp_long3 *= CosAttitude; // apply attitude projection |
Line 1490... | Line 1490... | ||
1490 | tmp_long3 /= 8192; |
1490 | tmp_long3 /= 8192; |
Line 1524... | Line 1524... | ||
1524 | HooverGasMax = 1023; |
1524 | HooverGasMax = 1023; |
1525 | } |
1525 | } |
1526 | } |
1526 | } |
1527 | } |
1527 | } |
1528 | DebugOut.Analog[25] = HooverGas; |
1528 | DebugOut.Analog[25] = HooverGas; |
1529 | DebugOut.Analog[26] = HooverGasMax; |
1529 | //DebugOut.Analog[26] = HooverGasMax; |
1530 | DebugOut.Analog[28] = HooverGasMin; |
- | |
1531 | - | ||
1532 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1530 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
Line 1533... | Line 1531... | ||
1533 | 1531 | ||
1534 | // limit gas to parameter setting |
1532 | // limit gas to parameter setting |
1535 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1533 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
Line 1549... | Line 1547... | ||
1549 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1547 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1550 | DebugOut.Analog[7] = GasMischanteil; |
1548 | DebugOut.Analog[7] = GasMischanteil; |
1551 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1549 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1552 | // Gier-Anteil |
1550 | // Gier-Anteil |
1553 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1551 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1554 | #define MUL_G 1.0 |
- | |
1555 | GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
1552 | GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
1556 | // GierMischanteil = 0; |
- | |
1557 | #define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
1553 | #define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
1558 | if(GasMischanteil > MIN_GIERGAS) |
1554 | if(GasMischanteil > MIN_GIERGAS) |
1559 | { |
1555 | { |
1560 | if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; |
1556 | if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; |
1561 | if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); |
1557 | if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); |