Rev 1626 | Rev 1639 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1626 | Rev 1638 | ||
|---|---|---|---|
| Line 274... | Line 274... | ||
| 274 | // restore from eeprom |
274 | // restore from eeprom |
| 275 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
275 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
| 276 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
276 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
| 277 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
277 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
| 278 | // strange settings? |
278 | // strange settings? |
| 279 | if((NeutralAccX > 2048) || (NeutralAccY > 2048) || (NeutralAccZ > 1024)) |
279 | if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024)) |
| 280 | { |
280 | { |
| 281 | printf("\n\rACC not calibrated!\r\n"); |
281 | printf("\n\rACC not calibrated!\r\n"); |
| 282 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
282 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
| 283 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
283 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
| 284 | NeutralAccZ = Aktuell_az; |
284 | NeutralAccZ = Aktuell_az; |
| Line 498... | Line 498... | ||
| 498 | { |
498 | { |
| 499 | FCFlags &= ~(FCFLAG_MOTOR_RUN | FCFLAG_FLY); |
499 | FCFlags &= ~(FCFLAG_MOTOR_RUN | FCFLAG_FLY); |
| 500 | for(i=0;i<MAX_MOTORS;i++) |
500 | for(i=0;i<MAX_MOTORS;i++) |
| 501 | { |
501 | { |
| 502 | if(!PC_MotortestActive) MotorTest[i] = 0; |
502 | if(!PC_MotortestActive) MotorTest[i] = 0; |
| 503 | Motor[i].SetPoint = MotorTest[i]; |
503 | Motor[i].SetPoint = MotorTest[i]; |
| - | 504 | Motor[i].SetPointLowerBits = 0; |
|
| - | 505 | /* |
|
| - | 506 | Motor[i].SetPoint = MotorTest[i] / 4; |
|
| - | 507 | Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
|
| - | 508 | */ |
|
| 504 | } |
509 | } |
| 505 | if(PC_MotortestActive) PC_MotortestActive--; |
510 | if(PC_MotortestActive) PC_MotortestActive--; |
| 506 | } |
511 | } |
| 507 | else FCFlags |= FCFLAG_MOTOR_RUN; |
512 | else FCFlags |= FCFLAG_MOTOR_RUN; |
| Line 1206... | Line 1211... | ||
| 1206 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
1211 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
| 1207 | DebugOut.Analog[8] = KompassValue; |
1212 | DebugOut.Analog[8] = KompassValue; |
| 1208 | DebugOut.Analog[9] = UBat; |
1213 | DebugOut.Analog[9] = UBat; |
| 1209 | DebugOut.Analog[10] = SenderOkay; |
1214 | DebugOut.Analog[10] = SenderOkay; |
| 1210 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1215 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
| 1211 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1216 | //DebugOut.Analog[16] = Motor[0].Temperature; |
| 1212 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1217 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
| 1213 | DebugOut.Analog[18] = VarioMeter; |
1218 | // DebugOut.Analog[18] = VarioMeter; |
| 1214 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1219 | // DebugOut.Analog[19] = WinkelOut.CalcState; |
| 1215 | DebugOut.Analog[20] = ServoNickValue; |
1220 | DebugOut.Analog[20] = ServoNickValue; |
| 1216 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
1221 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
| 1217 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
1222 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
| 1218 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
1223 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
| 1219 | // DebugOut.Analog[24] = MesswertNick/2; |
1224 | // DebugOut.Analog[24] = MesswertNick/2; |
| Line 1263... | Line 1268... | ||
| 1263 | // if height control is activated |
1268 | // if height control is activated |
| 1264 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1269 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
| 1265 | { |
1270 | { |
| 1266 | #define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1271 | #define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| 1267 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
1272 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
| - | 1273 | ||
| - | 1274 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
|
| - | 1275 | #define OPA_OFFSET_STEP 15 |
|
| - | 1276 | #else |
|
| 1268 | #define OPA_OFFSET_STEP 10 |
1277 | #define OPA_OFFSET_STEP 10 |
| - | 1278 | #endif |
|
| 1269 | int HCGas, HeightDeviation = 0; |
1279 | int HCGas, HeightDeviation = 0; |
| 1270 | static int HeightTrimming = 0; // rate for change of height setpoint |
1280 | static int HeightTrimming = 0; // rate for change of height setpoint |
| 1271 | static int FilterHCGas = 0; |
1281 | static int FilterHCGas = 0; |
| 1272 | static int StickGasHover = 120, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
1282 | static int StickGasHover = 120, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
| 1273 | static unsigned long HoverGasFilter = 0; |
1283 | static unsigned long HoverGasFilter = 0; |
| 1274 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1284 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| 1275 | int CosAttitude; // for projection of hoover gas |
1285 | int CosAttitude; // for projection of hoover gas |
| Line 1276... | Line 1286... | ||
| 1276 | 1286 | ||
| 1277 | // get the current hooverpoint |
1287 | // get the current hooverpoint |
| 1278 | DebugOut.Analog[21] = HoverGas; |
- | |
| 1279 | DebugOut.Analog[18] = VarioMeter; |
1288 | DebugOut.Analog[21] = HoverGas; |
| 1280 | 1289 | ||
| 1281 | // Expand the measurement |
1290 | // Expand the measurement |
| 1282 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
1291 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
| 1283 | if(!BaroExpandActive) |
1292 | if(!BaroExpandActive) |
| 1284 | { |
1293 | { |
| Line 1638... | Line 1647... | ||
| 1638 | tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
1647 | tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
| 1639 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
1648 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
| 1640 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
1649 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
| 1641 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1650 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
| 1642 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
1651 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
| 1643 | tmp_int = tmp_motorwert[i] / STICK_GAIN; |
1652 | tmp_int = tmp_motorwert[i] / 4; |
| - | 1653 | Motor[i].SetPointLowerBits = tmp_motorwert[i] % 4; |
|
| 1644 | LIMIT_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
1654 | LIMIT_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
| 1645 | Motor[i].SetPoint = tmp_int; |
1655 | Motor[i].SetPoint = tmp_int; |
| 1646 | } |
1656 | } |
| 1647 | else Motor[i].SetPoint = 0; |
1657 | else Motor[i].SetPoint = 0; |
| 1648 | } |
1658 | } |