| 162,14 → 162,7 |
|---|
| #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
| #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
| #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| uint8_t servoValues[6] = { 127, 127, 127, 127, 127 , 127 }; |
| |
| uint8_t MakeByte(int16_t i) { |
| if (i < 0) return 0; |
| if (i > 255) return 255; |
| return i; |
| } |
| |
| int MotorSmoothing(int neu, int alt) |
| { |
| int motor; |
| 478,10 → 471,21 |
|---|
| { |
| unsigned char i; |
| if(!MotorenEin) |
| { |
| MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
| else |
| MikroKopterFlags |= FLAG_MOTOR_RUN; |
| for(i=0;i<MAX_MOTORS;i++) |
| { |
| if(!PC_MotortestActive) MotorTest[i] = 0; |
| Motor[i] = MotorTest[i]; |
| } |
| if(PC_MotortestActive) PC_MotortestActive--; |
| } |
| else MikroKopterFlags |= FLAG_MOTOR_RUN; |
| |
| /* DebugOut.Analog[12] = Motor[0]; |
| DebugOut.Analog[13] = Motor[1]; |
| DebugOut.Analog[14] = Motor[3]; |
| DebugOut.Analog[15] = Motor[2];*/ |
| |
| //Start I2C Interrupt Mode |
| twi_state = 0; |
| 534,6 → 538,7 |
|---|
| MIN_GAS = EE_Parameter.Gas_Min; |
| } |
| |
| |
| //############################################################################ |
| // |
| void MotorRegler(void) |
| 592,7 → 597,6 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(SenderOkay > 140) |
| { |
| |
| MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
| RcLostTimer = EE_Parameter.NotGasZeit * 50; |
| if(GasMischanteil > 40 && MotorenEin) |
| 1164,7 → 1168,7 |
|---|
| DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
| DebugOut.Analog[4] = MesswertGier; |
| DebugOut.Analog[5] = HoehenWert/5; |
| //DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
| DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
| DebugOut.Analog[8] = KompassValue; |
| DebugOut.Analog[9] = UBat; |
| DebugOut.Analog[10] = SenderOkay; |
| 1525,7 → 1529,7 |
|---|
| modell_fliegt = 1; |
| GasMischanteil = MIN_GAS; |
| } |
| /*// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Mischer und PI-Regler |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DebugOut.Analog[7] = GasMischanteil; |
| 1573,69 → 1577,39 |
|---|
| pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
| if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;*/ |
| if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Servo Mixer |
| // Universal Mixer |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| { |
| int16_t nick, roll, tmp, pitch, throttle; |
| int16_t servo[6]; |
| #if 1 |
| |
| // throttle |
| throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + Mixer.Motor[2][0]; |
| if (throttle < 0) throttle = 0; |
| #include "helimixer.inc"; |
| |
| tmp = throttle / Mixer.Motor[3][0]; |
| if (tmp > Mixer.Motor[3][1] * 2) tmp = Mixer.Motor[3][1] * 2; |
| DebugOut.Analog[7] = tmp; |
| if (MotorenEin == 0 || MikroKopterFlags & FLAG_NOTLANDUNG) { |
| tmp = 0; |
| } |
| servo[4] = tmp; |
| #else |
| |
| // pitch |
| pitch = throttle / Mixer.Motor[4][0] + Mixer.Motor[4][2]; |
| if (pitch > Mixer.Motor[4][1]) pitch = Mixer.Motor[4][1]; |
| for(i=0; i<MAX_MOTORS; i++) |
| { |
| signed int tmp_int; |
| if(Mixer.Motor[i][0] > 0) |
| { |
| tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
| tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
| tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
| tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
| tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
| tmp_int = tmp_motorwert[i] / STICK_GAIN; |
| CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
| Motor[i] = tmp_int; |
| } |
| else Motor[i] = 0; |
| } |
| /* |
| if(Poti1 > 20) Motor1 = 0; |
| if(Poti1 > 90) Motor6 = 0; |
| if(Poti1 > 140) Motor2 = 0; |
| //if(Poti1 > 200) Motor7 = 0; |
| */ |
| #endif |
| |
| servo[0] = -pitch; |
| servo[1] = pitch; |
| servo[2] = pitch; |
| |
| |
| nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
| roll = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
| |
| // nick |
| nick /= Mixer.Motor[0][1]; |
| nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / Mixer.Motor[5][0]; |
| servo[0] += 127 + Mixer.Motor[0][0] - nick; |
| servo[1] += 127 + Mixer.Motor[0][0] + nick; |
| servo[2] += 127 + Mixer.Motor[0][0] - nick; |
| |
| // roll |
| roll /= Mixer.Motor[0][2]; |
| roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / Mixer.Motor[6][0]; |
| servo[0] += roll; |
| servo[1] += roll; |
| |
| // gier |
| tmp = MesswertGier / Mixer.Motor[0][3]; |
| servo[3] = 127 + Mixer.Motor[1][1] + tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] / Mixer.Motor[7][0]); |
| |
| // not used |
| servo[5] = 127; |
| |
| for(tmp = 0; tmp < 6; tmp++) { |
| servoValues[tmp] = MakeByte(servo[tmp]); |
| } |
| |
| DebugOut.Analog[12] = servo[0]; |
| DebugOut.Analog[13] = servo[1]; |
| DebugOut.Analog[14] = servo[2]; |
| DebugOut.Analog[15] = servo[3]; |
| DebugOut.Analog[6] = pitch; |
| |
| } |
| |
| } |