Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1210 → Rev 1211

/trunk/fc.c
86,8 → 86,8
long ErsatzKompass;
int ErsatzKompassInGrad; // Kompasswert in Grad
int GierGyroFehler = 0;
char GyroFaktor;
char IntegralFaktor;
char GyroFaktor,GyroFaktorGier;
char IntegralFaktor,IntegralFaktorGier;
int DiffNick,DiffRoll;
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links;
150,37 → 150,6
long GIER_GRAD_FAKTOR = 1291;
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
unsigned char RequiredMotors = 4;
 
/*
signed char Mixer.Motor[MAX_MOTORS][4] = {
{64, +64, 0, +64},//1
{64, +64, -64, -64},//2
{64, 0, -64, +64},//3
{64, -64, -64, -64},//4
{64, -64, 0, +64},//5
{64, -64, +64, -64},//6
{64, 0, +64, +64},//7
{64, +64, +64, -64},//8
{ 0, 0, 0, 0},//9
{ 0, 0, 0, 0},//10
{ 0, 0, 0, 0},//11
{ 0, 0, 0, 0}};//12
*/
/*
signed char Mixer.Motor[MAX_MOTORS][4] = {
{ 64, +64, 0, +64},//1
{ 64, -64, 0, +64},//2
{ 64, 0, -64, -64},//3
{ 64, 0, +64, -64},//4
{ 0, 0, 0, 0},//5
{ 0, 0, 0, 0},//6
{ 0, 0, 0, 0},//7
{ 0, 0, 0, 0},//8
{ 0, 0, 0, 0},//9
{ 0, 0, 0, 0},//10
{ 0, 0, 0, 0},//11
{ 0, 0, 0, 0}};//12
*/
unsigned char Motor[MAX_MOTORS];
signed int tmp_motorwert[MAX_MOTORS];
 
585,7 → 554,6
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
static int hoehenregler = 0;
// static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8;
static char TimerWerteausgabe = 0;
static char NeueKompassRichtungMerken = 0;
static long ausgleichNick, ausgleichRoll;
771,7 → 739,6
if(!NewPpmData-- || Notlandung)
{
int tmp_int;
static int stick_nick,stick_roll;
ParameterZuordnung();
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
787,16 → 754,10
if(StickGier < -2) StickGier += 2; else StickGier = 0;
 
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 
/* if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick)
MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--;
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--;
*/
// GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / (256.0/STICK_GAIN);
GyroFaktor = (Parameter_Gyro_P + 10.0);
// IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
IntegralFaktor = Parameter_Gyro_I;
GyroFaktorGier = (Parameter_Gyro_P + 10.0);
IntegralFaktorGier = Parameter_Gyro_I;
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
884,8 → 845,10
StickGier = 0;
StickNick = 0;
StickRoll = 0;
GyroFaktor = 90;//(float) 100 / (256.0 / STICK_GAIN);
IntegralFaktor = 120;//(float) 120 / (44000 / STICK_GAIN);
GyroFaktor = 90;
IntegralFaktor = 120;
GyroFaktorGier = 90;
IntegralFaktorGier = 120;
Looping_Roll = 0;
Looping_Nick = 0;
}
1175,14 → 1138,6
{
fehler = 0;
}
 
if(NeueKompassRichtungMerken)
{
// ErsatzKompass += (fehler * 32) / korrektur;
// fehler = 0;
// fehler /= 4;
// ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
}
if(!KompassSignalSchlecht && w < 25)
{
GierGyroFehler += fehler;
1289,17 → 1244,10
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
 
{
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
}
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
 
#ifndef QUADRO
MesswertGier = (long)(MesswertGier * 4 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (4 * (44000 / STICK_GAIN));
#else
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktor) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktor) / (2 * (44000 / STICK_GAIN));
#endif
 
// Maximalwerte abfangen
// #define MAX_SENSOR (4096*STICK_GAIN)
#define MAX_SENSOR (4096*4)
1376,7 → 1324,6
{
h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
h = GasMischanteil - h; // vom Gas abziehen
// h -= (HoeheD * Parameter_Luftdruck_D)/(8/STICK_GAIN); // D-Anteil
h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil
tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN;
/trunk/fc.h
54,9 → 54,6
void Mittelwert(void);
void SetNeutral(void);
void Piep(unsigned char Anzahl);
extern void DefaultKonstanten(void);
void DefaultKonstanten1(void);
void DefaultKonstanten2(void);
 
extern unsigned char h,m,s;
extern volatile unsigned char Timeout ;
71,6 → 68,8
extern char MotorenEin;
extern void DefaultKonstanten1(void);
extern void DefaultKonstanten2(void);
extern void DefaultKonstanten3(void);
extern void DefaultStickMapping(void);
 
#define STRUCT_PARAM_LAENGE sizeof(EE_Parameter)
struct mk_param_struct
156,6 → 155,7
 
struct
{
char Revision;
char Name[12];
signed char Motor[16][4];
} Mixer;
/trunk/main.c
121,7 → 121,7
int main (void)
//############################################################################
{
unsigned int timer;
unsigned int timer,i;
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++); // verzögern
165,6 → 165,7
 
sei();
 
printf("\n\r===================================");
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a');
if(UCSR1A == 0x20 && UCSR1C == 0x06) // initial Values for 644P
{
173,7 → 174,7
GRN_ON;
ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes
 
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_REVISION]) == EE_MIXER_REVISION)
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) // Check Revision in the first Byte
{
unsigned char i;
RequiredMotors = 0;
183,6 → 184,7
else // default
{
unsigned char i;
printf("\n\rGenerating default Mixer Table");
for(i=0; i<16;i++) { Mixer.Motor[i][0] = 0;Mixer.Motor[i][1] = 0;Mixer.Motor[i][2] = 0;Mixer.Motor[i][3] = 0;};
// default = Quadro
Mixer.Motor[0][0] = 64; Mixer.Motor[0][1] = +64; Mixer.Motor[0][2] = 0; Mixer.Motor[0][3] = +64;
189,12 → 191,32
Mixer.Motor[1][0] = 64; Mixer.Motor[1][1] = -64; Mixer.Motor[1][2] = 0; Mixer.Motor[1][3] = +64;
Mixer.Motor[2][0] = 64; Mixer.Motor[2][1] = 0; Mixer.Motor[2][2] = -64; Mixer.Motor[2][3] = -64;
Mixer.Motor[3][0] = 64; Mixer.Motor[3][1] = 0; Mixer.Motor[3][2] = +64; Mixer.Motor[3][3] = -64;
Mixer.Revision = MIXER_REVISION;
memcpy(Mixer.Name, "Quadro\0", 11);
eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_REVISION],EE_MIXER_REVISION); // Länge der Datensätze merken
}
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name,RequiredMotors);
printf("\n\r==============================");
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Check connected BL-Ctrls
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
printf("\n\rFound BL-Ctrl: ");
motorread = 0; UpdateMotor = 0; SendMotorData(); while(!UpdateMotor); motorread = 0; // read the first I2C-Data
for(i=0; i < MAX_MOTORS; i++)
{
UpdateMotor = 0;
SendMotorData();
while(!UpdateMotor);
if(MotorPresent[i]) printf("%d ",i+1);
}
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);
MotorError[i] = 0;
}
printf("\n\r===================================");
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Check Settings
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
{
DefaultKonstanten1();
233,16 → 255,16
 
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
{
printf("\n\rACC nicht abgeglichen!");
printf("\n\rACC not calibrated !");
}
 
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
printf("\n\rUsing parameterset %d", GetActiveParamSetNumber());
 
 
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
{
printf("\n\rAbgleich Luftdrucksensor..");
printf("\n\rCalibrating pressure sensor..");
timer = SetDelay(1000);
SucheLuftruckOffset();
while (!CheckDelay(timer));
257,11 → 279,11
ExternControl.Digital[0] = 0x55;
 
 
printf("\n\rSteuerung: ");
printf("\n\rControl: ");
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral");
else printf("Normal (ACC-Mode)");
 
printf("\n\n\r");
printf("\n\r===================================\n\r");
 
LcdClear();
I2CTimeout = 5000;
/trunk/main.h
68,8 → 68,8
//#error ################## F_CPU nicht definiert oder ungültig #############
//#endif
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define EE_DATENREVISION 75 // wird angepasst, wenn sich die EEPROM-Daten geändert haben
#define EE_MIXER_REVISION 10 // wird angepasst, wenn sich die Mixer-Daten geändert haben
#define EE_DATENREVISION 75 // wird angepasst, wenn sich die EEPROM-Daten geändert haben
#define MIXER_REVISION 1 // wird angepasst, wenn sich die Mixer-Daten geändert haben
 
#define EEPROM_ADR_VALID 1
#define EEPROM_ADR_ACTIVE_SET 2
84,8 → 84,7
#define EEPROM_ADR_PARAM_LENGTH 98
#define EEPROM_ADR_PARAM_BEGIN 100
 
#define EEPROM_ADR_MIXER_REVISION 1000
#define EEPROM_ADR_MIXER_TABLE 1001 // 1001 - 1100
#define EEPROM_ADR_MIXER_TABLE 1000 // 1001 - 1100
 
#define CFG_HOEHENREGELUNG 0x01
#define CFG_HOEHEN_SCHALTER 0x02
/trunk/makefile
5,7 → 5,7
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 73
VERSION_PATCH = 1
VERSION_PATCH = 2
 
VERSION_SERIAL_MAJOR = 10 # Serial Protocol
VERSION_SERIAL_MINOR = 0 # Serial Protocol
/trunk/menu.c
12,7 → 12,7
char DisplayBuff[80] = "Hallo Welt";
unsigned char DispPtr = 0;
 
unsigned char MaxMenue = 12;
unsigned char MaxMenue = 13;
unsigned char MenuePunkt = 0;
unsigned char RemoteKeys = 0;
 
146,6 → 146,12
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Hight,ExternControl.Config);
break;
case 12:
LCD_printfxy(0,0,"BL-Ctrl Errors " );
LCD_printfxy(0,1," %3d %3d %3d %3d ",MotorError[0],MotorError[1],MotorError[2],MotorError[3]);
LCD_printfxy(0,2," %3d %3d %3d %3d ",MotorError[4],MotorError[5],MotorError[6],MotorError[7]);
LCD_printfxy(0,3," %3d %3d %3d %3d ",MotorError[8],MotorError[9],MotorError[10],MotorError[11]);
break;
case 13:
LCD_printfxy(0,0,"BL-Ctrl found " );
LCD_printfxy(0,1," %c %c %c %c ",MotorPresent[0] + '-',MotorPresent[1] + '-',MotorPresent[2] + '-',MotorPresent[3] + '-');
LCD_printfxy(0,2," %c %c %c %c ",MotorPresent[4] + '-',MotorPresent[5] + '-',MotorPresent[6] + '-',MotorPresent[7] + '-');
154,7 → 160,6
if(MotorPresent[10]) LCD_printfxy(8,3,"11");
if(MotorPresent[11]) LCD_printfxy(12,3,"12");
break;
 
default: MaxMenue = MenuePunkt - 1;
MenuePunkt = 0;
break;
/trunk/timer0.h
5,6 → 5,7
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
 
void Timer_Init(void);
void TIMER2_Init(void);
void Delay_ms(unsigned int);
void Delay_ms_Mess(unsigned int);
unsigned int SetDelay (unsigned int t);
/trunk/twimaster.c
8,6 → 8,7
unsigned char motorread = 0,MissingMotor = 0;
unsigned char motor_rx[16],motor_rx2[16];
unsigned char MotorPresent[MAX_MOTORS];
unsigned char MotorError[MAX_MOTORS];
 
//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
95,7 → 96,7
SIGNAL (TWI_vect)
//############################################################################
{
static unsigned char missing_motor,cnt_motor;
static unsigned char missing_motor;
switch(twi_state++)
{
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
115,7 → 116,7
i2c_write_byte(Motor[motor++]);
break;
case 2:
if(TWSR == 0x30) { if(!missing_motor) missing_motor = motor; }
if(TWSR == 0x30) { if(!missing_motor) missing_motor = motor; if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;}
i2c_stop();
I2CTimeout = 10;
twi_state = 0;
125,22 → 126,22
// Reading Data
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
case 3:
//Transmit 1st byte for reading
 
//Transmit 1st byte for reading
if(TWSR != 0x40) // Error?
{
MotorPresent[motorread] = 0;
motorread++;
if(motorread >= MAX_MOTORS) motorread = 0;
i2c_stop();
{
MotorPresent[motorread] = 0;
motorread++;
if(motorread >= MAX_MOTORS) motorread = 0;
i2c_stop();
twi_state = 0;
}
else
{
else
{
MotorPresent[motorread] = ('1' - '-') + motorread;
}
I2C_ReceiveByte();
}
MissingMotor = missing_motor;
I2C_ReceiveByte();
missing_motor = 0;
break;
case 4: //Read 1st byte and transmit 2nd Byte
motor_rx[motorread] = TWDR;
149,10 → 150,11
case 5:
//Read 2nd byte
motor_rx2[motorread++] = TWDR;
if(motorread >= MAX_MOTORS) motorread = 0;
i2c_stop();
twi_state = 0;
missing_motor = 0;
break;
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// writing Gyro-Offset
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
212,141 → 214,3
}
TWCR |= 0x80;
}
/*
//############################################################################
SIGNAL (TWI_vect)
//############################################################################
{
switch (twi_state++)
{
case 0:
i2c_write_byte(0x52+(motor*2));
break;
case 1:
switch(motor++)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 2:
i2c_stop();
if (motor<4) twi_state = 0;
else motor = 0;
i2c_start();
break;
//Liest Daten von Motor
case 3:
i2c_write_byte(0x53+(motorread*2));
break;
case 4:
switch(motorread)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 5: //1 Byte vom Motor lesen
motor_rx[motorread] = TWDR;
 
case 6:
switch(motorread)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 7: //2 Byte vom Motor lesen
motor_rx[motorread+4] = TWDR;
motorread++;
if (motorread>3) motorread=0;
i2c_stop();
I2CTimeout = 10;
twi_state = 0;
break;
case 8: // Gyro-Offset
i2c_write_byte(0x98); // Address of the DAC
break;
case 9:
i2c_write_byte(0x10); // Update Channel A
break;
case 10:
i2c_write_byte(AnalogOffsetNick); // Value
break;
case 11:
i2c_write_byte(0x80); // Value
break;
case 12:
i2c_stop();
I2CTimeout = 10;
i2c_start();
break;
case 13:
i2c_write_byte(0x98); // Address of the DAC
break;
case 14:
i2c_write_byte(0x12); // Update Channel B
break;
case 15:
i2c_write_byte(AnalogOffsetRoll); // Value
break;
case 16:
i2c_write_byte(0x80); // Value
break;
case 17:
i2c_stop();
I2CTimeout = 10;
i2c_start();
break;
case 18:
i2c_write_byte(0x98); // Address of the DAC
break;
case 19:
i2c_write_byte(0x14); // Update Channel C
break;
case 20:
i2c_write_byte(AnalogOffsetGier); // Value
break;
case 21:
i2c_write_byte(0x80); // Value
break;
case 22:
i2c_stop();
I2CTimeout = 10;
twi_state = 0;
break;
}
TWCR |= 0x80;
}
*/
/trunk/twimaster.h
23,6 → 23,7
extern unsigned char motorread;
extern unsigned char motor_rx[];
extern unsigned char MotorPresent[];
extern unsigned char MotorError[];
 
void i2c_reset(void);
extern void i2c_init (void); // I2C initialisieren
/trunk/uart.c
312,11 → 312,15
break;
 
case 'm':// "Write Mixer
memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer));
eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_REVISION],EE_MIXER_REVISION); // Länge der Datensätze merken
while(!UebertragungAbgeschlossen);
SendOutData('M', FC_ADDRESS, 1, 1, 1);
if(pRxData[0] == MIXER_REVISION)
{
memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer));
eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
tempchar1 = 1;
}
else tempchar1 = 0;
SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
break;
 
case 'p': // get PPM Channels
/trunk/uart.h
20,7 → 20,7
extern int uart_putchar (char c);
extern void boot_program_page (uint32_t page, uint8_t *buf);
extern void DatenUebertragung(void);
extern void DecodeNMEA(void);
extern void Uart1Init(void);
extern void BearbeiteRxDaten(void);
extern unsigned char MotorTest[4];
struct str_DebugOut
/trunk/version.txt
243,3 → 243,8
 
0.73b: H.Buss 14.03.2009
- Es wird geprüft, ob alle notwendigen BL-Regler angeschlossen sind
 
0.73c: H.Buss 16.03.2009
- die Busfehler der BL-Regler werden im Menü angezeigt
- Revision der MixerTabelle eingeführt