| /trunk/Spectrum.c |
|---|
| 4,6 → 4,7 |
| #include "Spectrum.h" |
| #include "main.h" |
| unsigned char SpektrumTimer = 0; |
| //--------------------------------------------------------------// |
| 90,10 → 91,7 |
| UCSR1A |= (1 << U2X1); |
| // enable receiver and transmitter |
| //UCSR1B = (1<<RXEN1)|(1<<TXEN1); |
| UCSR1B = (1<<RXEN1); |
| // set asynchronous mode |
| UCSR1C &= ~(1 << UMSEL11); |
| 185,25 → 183,25 |
| // |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #define MAX_FRAMEGAP 56 // 7ms |
| #define MAX_BYTEGAP 3 // 375us |
| //############################################################################ |
| //Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever |
| SIGNAL(USART1_RX_vect) |
| //############################################################################ |
| { |
| static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer; |
| static unsigned char Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0; |
| unsigned int Channel, index; |
| signed int signal, tmp; |
| int bCheckDelay; |
| uint8_t c; |
| c = UDR1; // get data byte |
| if (ReSync == 1) |
| if(ReSync == 1) |
| { |
| // wait for beginning of new frame |
| ReSync = 0; |
| FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames |
| SpektrumTimer = MAX_FRAMEGAP; |
| FrameCnt = 0; |
| Sync = 0; |
| ByteHigh = 0; |
| 210,7 → 208,7 |
| } |
| else |
| { |
| bCheckDelay = CheckDelay(FrameTimer); |
| if(!SpektrumTimer) bCheckDelay = 1; else bCheckDelay = 0;//CheckDelay(FrameTimer); |
| if ( Sync == 0 ) |
| { |
| if(bCheckDelay) |
| 219,11 → 217,16 |
| // Zeichen ignorieren, da Bedeutung unbekannt |
| Sync = 1; |
| FrameCnt ++; |
| SpektrumTimer = MAX_BYTEGAP; |
| } |
| else |
| { |
| // Zeichen kam vor Ablauf der 7ms Sync-Pause |
| // warten auf erstes Sync-Zeichen |
| SpektrumTimer = MAX_FRAMEGAP; |
| FrameCnt = 0; |
| Sync = 0; |
| ByteHigh = 0; |
| } |
| } |
| else if((Sync == 1) && !bCheckDelay) |
| 231,12 → 234,13 |
| // zweites Sync-Character ignorieren, Bedeutung unbekannt |
| Sync = 2; |
| FrameCnt ++; |
| SpektrumTimer = MAX_BYTEGAP; |
| } |
| else if((Sync == 2) && !bCheckDelay) |
| { |
| SpektrumTimer = MAX_BYTEGAP; |
| // Datenbyte high |
| ByteHigh = c; |
| if (FrameCnt == 2) |
| { |
| // is 1st Byte of Channel-data |
| 254,18 → 258,17 |
| else if((Sync == 3) && !bCheckDelay) |
| { |
| // Datenbyte low |
| // High-Byte for next channel comes next |
| SpektrumTimer = MAX_BYTEGAP; |
| Sync = 2; |
| FrameCnt ++; |
| index = (ByteHigh >> 2) & 0x0f; |
| index ++; |
| Channel = (ByteHigh << 8) | c; |
| index++; |
| Channel = ((unsigned int)ByteHigh << 8) | c; |
| signal = Channel & 0x3ff; |
| signal -= 0x200; // Offset, range 0x000..0x3ff? |
| signal = signal/3; // scaling to fit PPM resolution |
| if(index >= 0 && index <= 10) |
| { |
| // Stabiles Signal |
| 285,6 → 288,7 |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; |
| } |
| else if(index > 17) ReSync = 1; // hier stimmt was nicht: neu synchronisieren |
| } |
| else |
| { |
| 292,9 → 296,11 |
| ReSync = 1; |
| FrameCnt = 0; |
| Frame2 = 0; |
| // new frame next, nach fruehestens 7ms erwartet |
| SpektrumTimer = MAX_FRAMEGAP; |
| } |
| // 16 Bytes per frame |
| // 16 Bytes eingetroffen -> Komplett |
| if(FrameCnt >= 16) |
| { |
| // Frame complete |
| 302,16 → 308,13 |
| { |
| // Null bedeutet: Neue Daten |
| // nur beim ersten Frame (CH 0-7) setzen |
| NewPpmData = 0; |
| if(!ReSync) NewPpmData = 0; |
| } |
| // new frame next, nach fruehestens 7ms erwartet |
| FrameCnt = 0; |
| Frame2 = 0; |
| Sync = 0; |
| SpektrumTimer = MAX_FRAMEGAP; |
| } |
| // Zeit bis zum nächsten Zeichen messen |
| FrameTimer = SetDelay(7); |
| } |
| } |
| /trunk/Spectrum.h |
|---|
| 6,4 → 6,5 |
| #define _SPECTRUM_H |
| void Uart1Init(void); |
| void SpektrumBinding(void); |
| extern unsigned char SpektrumTimer; |
| #endif //_RC_H |
| /trunk/analog.c |
|---|
| 112,7 → 112,6 |
| static long tmpLuftdruck = 0; |
| static char messanzahl_Druck = 0; |
| static long SummenHoehe = 0; |
| switch(state++) |
| { |
| case 0: |
| 244,6 → 243,7 |
| } |
| ADMUX = kanal; |
| if(state != 0) ANALOG_ON; |
| } |
| /trunk/eeprom.c |
|---|
| 19,7 → 19,7 |
| void DefaultKonstanten1(void) |
| { |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;//CFG_HOEHEN_SCHALTER |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER; |
| EE_Parameter.ExtraConfig = CFG2_HEIGHT_LIMIT;// | CFG2_VARIO_BEEP | CFG_SENSITIVE_RC |
| EE_Parameter.Hoehe_MinGas = 30; |
| EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
| 54,7 → 54,7 |
| EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
| EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoCompInvert = 1; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 6; |
| 100,7 → 100,7 |
| } |
| void DefaultKonstanten2(void) |
| { |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER; |
| EE_Parameter.ExtraConfig = CFG2_HEIGHT_LIMIT;// | CFG2_VARIO_BEEP | CFG_SENSITIVE_RC |
| EE_Parameter.Hoehe_MinGas = 30; |
| EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
| 135,7 → 135,7 |
| EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
| EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoCompInvert = 1; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 6; |
| 182,7 → 182,7 |
| void DefaultKonstanten3(void) |
| { |
| EE_Parameter.GlobalConfig = CFG_DREHRATEN_BEGRENZER | CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
| EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV | CFG_HOEHEN_SCHALTER; |
| EE_Parameter.ExtraConfig = CFG2_HEIGHT_LIMIT;// | CFG2_VARIO_BEEP | CFG_SENSITIVE_RC |
| EE_Parameter.Hoehe_MinGas = 30; |
| EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
| 217,7 → 217,7 |
| EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
| EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
| EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
| EE_Parameter.ServoCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoCompInvert = 1; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
| EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
| EE_Parameter.ServoNickRefresh = 6; |
| /trunk/fc.c |
|---|
| 54,6 → 54,7 |
| #include "main.h" |
| #include "eeprom.c" |
| #include "mymath.h" |
| unsigned char h,m,s; |
| volatile unsigned int I2CTimeout = 100; |
| 183,7 → 184,7 |
| { |
| unsigned char i; |
| unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
| ServoActive = 0; HEF4017R_ON; |
| HEF4017R_ON; |
| NeutralAccX = 0; |
| NeutralAccY = 0; |
| NeutralAccZ = 0; |
| 260,8 → 261,12 |
| Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110; |
| Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110; |
| Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110; |
| // ServoActive = 1; |
| SenderOkay = 100; |
| if(ServoActive) |
| { |
| HEF4017R_ON; |
| DDRD |=0x80; // enable J7 -> Servo signal |
| } |
| } |
| //############################################################################ |
| 645,6 → 650,7 |
| { |
| if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| } |
| ServoActive = 0; |
| SetNeutral(); |
| ServoActive = 1; |
| DDRD |=0x80; // enable J7 -> Servo signal |
| 1251,15 → 1257,6 |
| if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // all BL-Ctrl connected? |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
| { |
| modell_fliegt = 1; |
| GasMischanteil = MIN_GAS; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Höhenregelung |
| // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1266,7 → 1263,6 |
| if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
| GasMischanteil *= STICK_GAIN; |
| // if height control is activated |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
| { |
| 1276,11 → 1272,12 |
| int HCGas, HeightDeviation; |
| static int HeightTrimming = 0; // rate for change of height setpoint |
| static int FilterHCGas = 0; |
| static int HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static int StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static unsigned long HooverGasFilter = 0; |
| static unsigned char delay = 100; |
| 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}; |
| // 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[18] = VarioMeter; |
| 1327,7 → 1324,16 |
| 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; |
| tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
| tmp_long2 *= tmp_long2; |
| CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
| LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| #define HEIGHT_TRIM_UP 0x01 |
| 1334,7 → 1340,7 |
| #define HEIGHT_TRIM_DOWN 0x02 |
| static unsigned char HeightTrimmingFlag = 0x00; |
| #define HEIGHT_CONTROL_STICKTHRESHOLD 15 * STICK_GAIN |
| #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
| // Holger original version |
| // start of height control algorithm |
| // the height control is only an attenuation of the actual gas stick. |
| 1353,7 → 1359,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(GasMischanteil > (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
| { |
| 1361,9 → 1367,9 |
| SollHoehe = HoehenWert; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
| HeightTrimming += abs(GasMischanteil - (HooverGas + HEIGHT_CONTROL_STICKTHRESHOLD)) / 4; |
| HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
| } // gas stick is below hoover point |
| else if(GasMischanteil < (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) ) |
| { |
| if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
| { |
| 1371,7 → 1377,7 |
| SollHoehe = HoehenWert; // update setpoint to current heigth |
| } |
| HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
| HeightTrimming -= abs(GasMischanteil - (HooverGas - HEIGHT_CONTROL_STICKTHRESHOLD)) / 4; |
| HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
| } |
| else // Gas Stick in Hoover Range |
| { |
| 1389,9 → 1395,18 |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| 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; |
| } |
| } //if MikroKopterFlags & MKFLAG_FLY |
| else SollHoehe = HoehenWert - 200; |
| else |
| { |
| SollHoehe = HoehenWert - 200; |
| StickGasHoover = 110; |
| } |
| HCGas = HooverGas; // take hoover gas (neutral point) |
| } |
| 1427,11 → 1442,17 |
| HCGas -= tmp_int; |
| // strech control output by inverse attitude projection |
| // + 1/cos(angle) ++++++++++++++++++++++++++ |
| // strech control output by inverse attitude projection 1/cos |
| 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 |
| 1444,6 → 1465,12 |
| GasMischanteil = FilterHCGas; |
| } |
| }// EOF height control active |
| 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; |
| } |
| // Hoover gas estimation by averaging gas control output on small z-velocities |
| // this is done only if height contol option is selected in global config and aircraft is flying |
| 1452,12 → 1479,16 |
| 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 = (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; |
| // average vertical projected thrust |
| if(modell_fliegt < 2000) // the first 4 seconds |
| { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
| 1501,11 → 1532,19 |
| }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| // limit gas to parameter setting |
| LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
| LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
| if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // all BL-Ctrl connected? |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(MissingMotor) |
| if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
| { |
| modell_fliegt = 1; |
| GasMischanteil = MIN_GAS; |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Mischer und PI-Regler |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DebugOut.Analog[7] = GasMischanteil; |
| /trunk/main.c |
|---|
| 248,10 → 248,15 |
| } |
| for(i=0; i < MAX_MOTORS; i++) |
| { |
| if(!MotorPresent[i] && Mixer.Motor[i][0] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
| if(!MotorPresent[i] && Mixer.Motor[i][0] > 0) |
| { |
| printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
| ServoActive = 1; // just in case the FC would be used as camera-stabilizer |
| } |
| MotorError[i] = 0; |
| } |
| printf("\n\r==================================="); |
| printf("\n\r==================================="); |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Check Settings |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 293,7 → 298,7 |
| FlugMinuten = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES2]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1]); |
| FlugMinutenGesamt = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_MINUTES+1]); |
| if(FlugMinutenGesamt == 0xffff) |
| if(FlugMinutenGesamt == 0xffff || FlugMinuten == 0xffff) |
| { |
| FlugMinuten = 0; |
| FlugMinutenGesamt = 0; |
| 335,6 → 340,8 |
| WinkelOut.Orientation = 1; |
| LipoDetection(1); |
| printf("\n\r===================================\n\r"); |
| //SpektrumBinding(); |
| timer = SetDelay(100); |
| while (1) |
| { |
| if(UpdateMotor && AdReady) // ReglerIntervall |
| 383,6 → 390,7 |
| else BearbeiteRxDaten(); |
| if(CheckDelay(timer)) |
| { |
| timer += 20;//SetDelay(20); |
| if(PcZugriff) PcZugriff--; |
| else |
| { |
| 408,8 → 416,8 |
| else MikroKopterFlags &= ~FLAG_LOWBAT; |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| if(!MotorenEin) timer2 = 1300; // 0,5 Minuten aufrunden |
| if(++timer2 == 2600) // eine Minute |
| if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden |
| if(++timer2 == 2930) // eine Minute |
| { |
| timer2 = 0; |
| FlugMinuten++; |
| 419,7 → 427,6 |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES],FlugMinutenGesamt / 256); // ACC-NeutralWerte speichern |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES+1],FlugMinutenGesamt % 256); // ACC-NeutralWerte speichern |
| } |
| timer = SetDelay(20); |
| } |
| LED_Update(); |
| } |
| /trunk/makefile |
|---|
| 5,7 → 5,7 |
| #------------------------------------------------------------------- |
| VERSION_MAJOR = 0 |
| VERSION_MINOR = 75 |
| VERSION_PATCH = 11 |
| VERSION_PATCH = 12 |
| VERSION_SERIAL_MAJOR = 10 # Serial Protocol |
| VERSION_SERIAL_MINOR = 1 # Serial Protocol |
| 104,6 → 104,7 |
| # List C source files here. (C dependencies are automatically generated.) |
| SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c |
| SRC += twimaster.c rc.c fc.c GPS.c spi.c led.c Spectrum.c |
| SRC += mymath.c |
| ########################################################################################################## |
| /trunk/menu.c |
|---|
| 82,9 → 82,9 |
| break; |
| case 4: |
| LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_NICK]],PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); |
| LCD_printfxy(0,1,"Gs:%4i Gi:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_GAS]],PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
| LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]]); |
| LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]]); |
| LCD_printfxy(0,1,"Gs:%4i Gi:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]+120,PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
| LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]]+120,PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]]+120); |
| LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]]+120,PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]]+120); |
| break; |
| case 5: |
| LCD_printfxy(0,0,"Gyro - Sensor"); |
| /trunk/mymath.c |
|---|
| 0,0 → 1,104 |
| #include <stdlib.h> |
| #include <avr/pgmspace.h> |
| #include "mymath.h" |
| // discrete mathematics |
| // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
| int16_t c_sin_8192(int16_t angle) |
| { |
| int8_t m,n; |
| int16_t sinus; |
| // avoid negative angles |
| if (angle < 0) |
| { |
| m = -1; |
| angle = abs(angle); |
| } |
| else m = +1; |
| // fold angle to intervall 0 to 359 |
| angle %= 360; |
| // check quadrant |
| if (angle <= 90) n=1; // first quadrant |
| else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| else {angle = 360 - angle; n = -1;} //fourth quadrant |
| // get lookup value |
| sinus = pgm_read_word(&pgm_sinlookup[angle]); |
| // calculate sinus value |
| return (sinus * m * n); |
| } |
| // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| int16_t c_cos_8192(int16_t angle) |
| { |
| return (c_sin_8192(90 - angle)); |
| } |
| // Arcustangens returns degree in a range of +/. 180 deg |
| const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
| int16_t c_atan2(int16_t y, int16_t x) |
| { |
| int16_t index, angle; |
| int8_t m; |
| if (!x && !y) return 0; //atan2(0, 0) is undefined |
| if (y < 0) m = -1; |
| else m = 1; |
| if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
| index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
| if (index < 0) index = -index; |
| if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
| else if (index > 7334) angle = 90; // limit is 90 deg |
| else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
| else if (index > 1465) angle = 88; |
| else if (index > 1046) angle = 87; |
| else if (index > 813) angle = 86; |
| else if (index > 664) angle = 85; |
| else if (index > 561) angle = 84; |
| else if (index > 486) angle = 83; |
| else if (index > 428) angle = 82; |
| else if (index > 382) angle = 81; |
| else angle = 80; // (index>345) |
| if (x > 0) return (angle * m); // 1st and 4th quadrant |
| else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
| else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
| } |
| // Integer square root |
| // For details of the algorithm see the article http://www.embedded.com/98/9802fe2.htm |
| uint32_t c_sqrt(uint32_t a) |
| { |
| uint32_t rem = 0; |
| uint32_t root = 0; |
| uint8_t i; |
| for(i = 0; i < 16; i++) |
| { |
| root <<= 1; |
| rem = ((rem << 2) + (a >> 30)); |
| a <<= 2; |
| root++; |
| if(root <= rem) |
| { |
| rem -= root; |
| root++; |
| } |
| else root--; |
| } |
| return (root >> 1); |
| } |
| /trunk/mymath.h |
|---|
| 0,0 → 1,11 |
| #ifndef _MYMATH_H |
| #define _MYMATH_H |
| #include <inttypes.h> |
| extern int16_t c_sin_8192(int16_t angle); |
| extern int16_t c_cos_8192(int16_t angle); |
| extern int16_t c_atan2(int16_t y, int16_t x); |
| extern uint32_t c_sqrt(uint32_t a); |
| #endif // _MYMATH_H |
| /trunk/rc.c |
|---|
| 23,7 → 23,6 |
| //############################################################################ |
| { |
| TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);//|(1 << WGM12); //timer1 prescale 64 |
| // TCCR1B=(1<<CS11)|(0<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64 |
| TIMSK1 |= _BV(ICIE1); |
| AdNeutralGier = 0; |
| AdNeutralRoll = 0; |
| 47,6 → 46,7 |
| //Syncronisationspause? (3.52 ms < signal < 25.6 ms) |
| if((signal > 1100) && (signal < 8000)) |
| { |
| Channels = index; |
| if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten |
| index = 1; |
| } |
| /trunk/spi.c |
|---|
| 159,8 → 159,6 |
| void UpdateSPI_Buffer(void) |
| { |
| signed int tmp; |
| cli(); |
| ToNaviCtrl.IntegralNick = (int) (IntegralNick / (long)(EE_Parameter.GyroAccFaktor * 4)); |
| ToNaviCtrl.IntegralRoll = (int) (IntegralRoll / (long)(EE_Parameter.GyroAccFaktor * 4)); |
| ToNaviCtrl.GyroCompass = (10 * ErsatzKompass) / GIER_GRAD_FAKTOR; |
| 169,7 → 167,6 |
| NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
| // ToNaviCtrl.User8 = Parameter_UserParam8; |
| // ToNaviCtrl.CalState = WinkelOut.CalcState; |
| switch(ToNaviCtrl.Command) // |
| { |
| case SPI_CMD_USER: |
| 204,6 → 201,7 |
| break; |
| case SPI_CMD_STICK: |
| cli(); |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| ToNaviCtrl.Param.Byte[0] = (char) tmp; |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| 211,6 → 209,7 |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| ToNaviCtrl.Param.Byte[2] = (char) tmp; |
| tmp = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
| sei(); |
| ToNaviCtrl.Param.Byte[3] = (char) tmp; |
| ToNaviCtrl.Param.Byte[4] = (unsigned char) Poti1; |
| ToNaviCtrl.Param.Byte[5] = (unsigned char) Poti2; |
| 218,9 → 217,8 |
| ToNaviCtrl.Param.Byte[7] = (unsigned char) Poti4; |
| ToNaviCtrl.Param.Byte[8] = (unsigned char) SenderOkay; |
| ToNaviCtrl.Param.Byte[9] = (unsigned char) SenderRSSI; |
| ToNaviCtrl.Param.Byte[10] = DebugOut.Analog[7] / 4; |
| ToNaviCtrl.Param.Byte[10] = DebugOut.Analog[7] / 4; //GasMischanteil |
| break; |
| case SPI_CMD_MISC: |
| if(WinkelOut.CalcState > 5) |
| { |
| 229,7 → 227,7 |
| } |
| else ToNaviCtrl.Param.Byte[0] = WinkelOut.CalcState; |
| ToNaviCtrl.Param.Byte[1] = EE_Parameter.NaviPH_LoginTime; |
| ToNaviCtrl.Param.Int[1] = (int)(HoehenWert/5); |
| ToNaviCtrl.Param.Int[1] = DebugOut.Analog[5];// = HoehenWert/5; |
| ToNaviCtrl.Param.Int[2] = (int)(SollHoehe/5); |
| ToNaviCtrl.Param.Byte[6] = EE_Parameter.NaviGpsPLimit; |
| ToNaviCtrl.Param.Byte[7] = EE_Parameter.NaviGpsILimit; |
| 258,8 → 256,6 |
| break; |
| } |
| sei(); |
| if(SPI_RxDataValid) |
| { |
| if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) |
| /trunk/timer0.c |
|---|
| 1,4 → 1,5 |
| #include "main.h" |
| #include "spectrum.h" |
| volatile unsigned int CountMilliseconds = 0; |
| volatile static unsigned int tim_main; |
| 30,8 → 31,9 |
| { |
| static unsigned char cnt_1ms = 1,cnt = 0; |
| unsigned char pieper_ein = 0; |
| // TCNT0 -= 250;//TIMER_RELOAD_VALUE; |
| if(SendSPI) SendSPI--; |
| if(SpektrumTimer) SpektrumTimer--; |
| if(!cnt--) |
| { |
| cnt = 9; |
| 80,12 → 82,13 |
| cntKompass += cntKompass / 41; |
| if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0; |
| } |
| // if(cntKompass < 10) cntKompass = 10; |
| // if(cntKompass < 10) cntKompass =r 10; |
| // KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L; |
| KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
| cntKompass = 0; |
| } |
| } |
| } |
| /trunk/version.txt |
|---|
| 311,3 → 311,8 |
| 0.75L H.Buss 23.09.2009 |
| - SollHoehe und Gas geht nun auch an die NC |
| 0.75M H.Buss 29.09.2009 |
| - Spektrum-Timing wird nun überwacht |
| - die FC kann nun stand-Alone als Kamera-Stabilizer eingesetzt werden, weil die Servos aktiviert werden, wenn I2C fehlt |