Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 377 → Rev 378

/branches/v0.60_MicroMag3_Nick666/trunc/analog.c
27,7 → 27,7
//#######################################################################################
{
ADMUX = 0;//Referenz ist extern
ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
//Free Running Mode, Division Factor 128, Interrupt on
}
 
/branches/v0.60_MicroMag3_Nick666/trunc/analog.h
20,4 → 20,4
 
 
#define ANALOG_OFF ADCSRA=0
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE)
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE)
/branches/v0.60_MicroMag3_Nick666/trunc/compass.c
15,7 → 15,7
 
#include "main.h"
 
struct MM3_calib_struct ee_calib EEMEM;
struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM
 
struct MM3_working_struct MM3;
struct MM3_calib_struct MM3_calib;
26,8 → 26,8
void init_MM3(void)
//############################################################################
{
SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator
//SPSR = (1<<SPI2X);
SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1); //Interrupt an, Master, 625 kHz Oszillator
SPSR = (1<<SPI2X);
DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang
60,22 → 60,13
else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30)
else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse
MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
MM3.DRDY = SetDelay(5); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms)
MM3.STATE = MM3_WAIT_DRDY;
return;
case MM3_WAIT_DRDY:
if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei
return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
/*
case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in °
MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8);
MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8);
MM3.AXIS = MM3_X;
MM3.STATE = MM3_RESET;
return;
*/
return; // Jetzt gehts weiter in SIGNAL (SIG_SPI)
}
}
 
168,7 → 159,7
{
ROT_FLASH;
GRN_FLASH;
//beeptime = 50;
beeptime = 50;
beeper = 50;
}
beeper--;
181,14 → 172,18
if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
}
// Offset der Achsen berechnen
MM3_calib.X_off = (x_max + x_min) / 2;
MM3_calib.Y_off = (y_max + y_min) / 2;
MM3_calib.Z_off = (z_max + z_min) / 2;
// Wertebereich der Achsen
MM3_calib.X_range = (x_max - x_min);
MM3_calib.Y_range = (y_max - y_min);
MM3_calib.Z_range = (z_max - z_min);
// Offset der Achsen
MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max;
MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max;
MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max;
 
// und im EEProm abspeichern
eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct));
}
 
 
198,27 → 193,55
//############################################################################
{
float sin_nick, cos_nick, sin_roll, cos_roll;
signed int x_corr, y_corr, heading;
signed int x_axis,y_axis,z_axis;
float x_corr, y_corr;
signed int x_axis,y_axis,z_axis, heading;
unsigned int div_faktor;
div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8;
MM3.NickGrad = -(IntegralNick/(EE_Parameter.UserParam1*8));
MM3.RollGrad = -(IntegralRoll/(EE_Parameter.UserParam2*8));
// Berechung von sinus und cosinus
MM3.NickGrad = -(IntegralNick/div_faktor);
sin_nick = sin_f(MM3.NickGrad);
cos_nick = cos_f(MM3.NickGrad);
MM3.RollGrad = -(IntegralRoll/div_faktor);
sin_roll = sin_f(MM3.RollGrad);
cos_roll = cos_f(MM3.RollGrad);
// Offset
x_axis = (MM3.x_axis + MM3_calib.X_off);
y_axis = (MM3.y_axis + MM3_calib.Y_off);
z_axis = (MM3.z_axis + MM3_calib.Z_off);
 
// Offset der Achsen nur bei Bedarf (also hier) berücksichtigen
x_axis = -(MM3.x_axis - MM3_calib.X_off);
y_axis = -(MM3.y_axis - MM3_calib.Y_off);
z_axis = -(MM3.z_axis - MM3_calib.Z_off);
// Normierung Wertebereich
if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range))
{
y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range;
z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range;
}
else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range))
{
x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range;
z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range;
}
else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range))
{
x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range;
y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range;
}
DebugOut.Analog[9] = x_axis;
DebugOut.Analog[10] = y_axis;
DebugOut.Analog[11] = z_axis;
// Neigungskompensation
x_corr = (cos_nick * x_axis) + (((sin_roll * y_axis) - (cos_roll * z_axis)) * sin_nick);
y_corr = ((cos_roll * y_axis) + (sin_roll * z_axis));
x_corr = x_axis * cos_nick;
x_corr += y_axis * sin_roll * sin_nick;
x_corr -= z_axis * cos_roll * sin_nick;
y_corr = y_axis * cos_roll;
y_corr += z_axis * sin_roll;
// Winkelberechnung
heading = atan2_i(x_corr, y_corr);
 
/branches/v0.60_MicroMag3_Nick666/trunc/compass.h
21,6 → 21,9
int8_t X_off;
int8_t Y_off;
int8_t Z_off;
uint16_t X_range;
uint16_t Y_range;
uint16_t Z_range;
};
 
extern struct MM3_working_struct MM3;
37,9 → 40,10
#define MM3_RESET 0
#define MM3_START_TRANSFER 1
#define MM3_WAIT_DRDY 2
#define MM3_DRDY 4
#define MM3_BYTE2 8
#define MM3_X 16
#define MM3_Y 32
#define MM3_Z 64
#define MM3_TILT 128
#define MM3_DRDY 3
#define MM3_BYTE2 4
#define MM3_X 5
#define MM3_Y 6
#define MM3_Z 7
#define MM3_TILT 8
#define MM3_IDLE 9
/branches/v0.60_MicroMag3_Nick666/trunc/fc.c
212,6 → 212,7
// ADC einschalten
ANALOG_ON;
 
/*
//------------------------------------------------------------------------------
if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
else
221,6 → 222,7
else
if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
//------------------------------------------------------------------------------
*/
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
688,9 → 690,7
DebugOut.Analog[7] = GasMischanteil;
DebugOut.Analog[8] = KompassValue;
 
DebugOut.Analog[9] = MM3.x_axis;
DebugOut.Analog[10] = MM3.y_axis;
DebugOut.Analog[11] = MM3.z_axis;
// Kanäle 9 bis 11 in compass.c
 
// DebugOut.Analog[9] = SollHoehe;
// DebugOut.Analog[10] = Mess_Integral_Gier / 128;
/branches/v0.60_MicroMag3_Nick666/trunc/main.c
139,6 → 139,8
}
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 1);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], 60);
calib_acc();
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct));
152,7 → 154,7
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 100 && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 100)
{
printf("\n\rKalibriere Kompass");
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) calib_MM3();
calib_MM3();
}
//Neutrallage kalibrieren?
/branches/v0.60_MicroMag3_Nick666/trunc/math.c
123,5 → 123,7
if (i < 0) {m=-1;i=abs(i);}
else m=1;
if (i > 200) i = 200;
return (pgm_read_byte(&pgm_asin[i]) * m);
}
/branches/v0.60_MicroMag3_Nick666/trunc/menu.c
27,7 → 27,7
 
void Menu(void)
{
static unsigned char MaxMenue = 11,MenuePunkt=0;
static unsigned char MaxMenue = 12,MenuePunkt=0;
if(RemoteTasten & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue; LcdClear(); }
if(RemoteTasten & KEY2) { MenuePunkt++; LcdClear(); }
55,8 → 55,7
{
LCD_printfxy(0,1,"Keine ");
LCD_printfxy(0,2,"Höhenregelung");
}
}
break;
case 2:
LCD_printfxy(0,0,"akt. Lage");
111,11 → 110,17
LCD_printfxy(0,3,"Range:%3i-%3i",EE_Parameter.ServoNickMin,EE_Parameter.ServoNickMax);
break;
case 11:
LCD_printfxy(0,0,"MM3 kalib.");
LCD_printfxy(0,0,"MM3 Off");
LCD_printfxy(0,1,"X_Offset: %3i",MM3_calib.X_off);
LCD_printfxy(0,2,"Y_Offset: %3i",MM3_calib.Y_off);
LCD_printfxy(0,3,"Z_Offset: %3i",MM3_calib.Z_off);
break;
case 12:
LCD_printfxy(0,0,"MM3 Range");
LCD_printfxy(0,1,"X_Range: %4i",MM3_calib.X_range);
LCD_printfxy(0,2,"Y_Range: %4i",MM3_calib.Y_range);
LCD_printfxy(0,3,"Z_Range: %4i",MM3_calib.Z_range);
break;
default: MaxMenue = MenuePunkt - 1;
MenuePunkt = 0;
break;
/branches/v0.60_MicroMag3_Nick666/trunc/timer0.c
19,7 → 19,7
};
 
 
SIGNAL (SIG_OVERFLOW0) // 8kHz
SIGNAL (SIG_OVERFLOW0) // 9,8kHz
{
static unsigned char cnt_1ms = 1,cnt = 0;
// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
26,7 → 26,7
 
if(!cnt--)
{
cnt = 9;
cnt = 10;
cnt_1ms++;
cnt_1ms %= 2;
if(!cnt_1ms) UpdateMotor = 1;
46,15 → 46,13
{
timer0_MM3(); // Kompass auslesen
 
if (!cntKompass--) // Aufruf mit ~10 Hz
if (!cntKompass--) // Aufruf mit 10 Hz
{
KompassValue = heading_MM3();
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 800;
cntKompass = 980;
}
}
 
}