/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; |
} |
} |
} |