Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 15 → Rev 16

/trunk/main.c
54,12 → 54,14
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
signed int OffsetN, OffsetR, OffsetZ;
 
signed int RawMagnet1a,RawMagnet1b; // raw AD-Data
signed int RawMagnet2a,RawMagnet2b;
signed int RawMagnet3a,RawMagnet3b;
signed int RawAccN = 0, RawAccR = 0, RawAccZ = 0;
signed int AccN = 0, AccR = 0, AccZ = 0;
signed int OffsetAccN = 0, OffsetAccR = 0, OffsetAccZ = 0;
signed int Xmin = 0, Xmax = 0; Ymin = 0, Ymax = 0; Zmin = 0, Zmax = 0;
signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur
signed int MagnetN,MagnetR,MagnetZ;
66,6 → 68,9
unsigned int PwmHeading = 0;
unsigned int PC_Connected = 0;
unsigned int Heading;
unsigned char ExternCalState = 0;
unsigned char AngleSource = 0;
unsigned char AccPresent = 0;
#include "main.h"
 
uint16_t eeXmin EEMEM = 0;
74,8 → 79,10
uint16_t eeYmax EEMEM = 0;
uint16_t eeZmin EEMEM = 0;
uint16_t eeZmax EEMEM = 0;
uint16_t eeNoffset EEMEM = 0;
uint16_t eeRoffset EEMEM = 0;
uint16_t eeZoffset EEMEM = 0;
 
 
//############################################################################
//
void Wait(unsigned char dauer)
104,9 → 111,31
{
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
int heading;
nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);
roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);
if(!AngleSource)
{
if(AccN > 125) nick_rad = M_PI / 2;
else
if(AccN < -125) nick_rad = -M_PI / 2;
else
{
nick_rad = asin((double) AccN / 125.0);
}
if(AccR > 125) roll_rad = M_PI / 2;
else
if(AccR < -125) roll_rad = -M_PI / 2;
else
{
roll_rad = asin((double) AccR / 125.0);
}
ExternData.Winkel[0] = (nick_rad * 1800.0) / M_PI;
ExternData.Winkel[1] = (roll_rad * 1800.0) / M_PI;
ExternData.Orientation = 1;
}
else
{
nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);
roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);
}
 
Cx = MagnetN;
Cy = MagnetR;
142,7 → 171,10
{
unsigned char cal;
if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
else
if(ExternCalState) cal = ExternCalState;
else cal = ExternData.CalState;
 
switch(cal)
{
case 0:
155,6 → 187,9
Ymax = -10000;
Zmin = 10000;
Zmax = -10000;
OffsetAccN = RawAccN;
OffsetAccR = RawAccR;
OffsetAccZ = RawAccZ;
LED_OFF;
break;
case 2:
182,8 → 217,12
eeprom_write_word(&eeYmax, Ymax);
eeprom_write_word(&eeZmin, Zmin);
eeprom_write_word(&eeZmax, Zmax);
eeprom_write_word(&eeNoffset, OffsetAccN);
eeprom_write_word(&eeRoffset, OffsetAccR);
eeprom_write_word(&eeZoffset, OffsetAccZ);
Delay_ms(2000);
}
ExternCalState = 0;
LED_ON;
break;
}
206,12 → 245,40
DebugOut.Analog[11] = Ymax;
DebugOut.Analog[12] = Zmin;
DebugOut.Analog[13] = Zmax;
DebugOut.Analog[14] = ExternData.CalState;
DebugOut.Analog[15] = Heading;
DebugOut.Analog[14] = Heading;
if(!ExternCalState) DebugOut.Analog[15] = ExternData.CalState;
else DebugOut.Analog[15] = ExternCalState;
DebugOut.Analog[16] = ExternData.UserParameter[0];
DebugOut.Analog[17] = ExternData.UserParameter[1];
DebugOut.Analog[18] = AccN;
DebugOut.Analog[19] = AccR;
DebugOut.Analog[20] = AccZ;
DebugOut.Analog[21] = RawAccN;
DebugOut.Analog[22] = RawAccR;
DebugOut.Analog[23] = RawAccZ;
DebugOut.Analog[24] = OffsetAccN;
DebugOut.Analog[25] = OffsetAccR;
DebugOut.Analog[26] = OffsetAccZ;
}
 
void AccMeasurement(void)
{
if(AccPresent)
{
RawAccN = (RawAccN + (signed int)MessAD(2))/2;
RawAccR = (RawAccR + (signed int)MessAD(3))/2;
RawAccZ = (RawAccZ + (signed int)MessAD(6))/2;
}
else
{
RawAccN = 0;
RawAccR = 0;
RawAccZ = 0;
}
AccR = (((signed int) RawAccN - OffsetAccN) + AccR * 7) / 8;
AccN = (((signed int) RawAccR - OffsetAccR) + AccN * 7) / 8;
AccZ = ((OffsetAccZ - (signed int) RawAccZ) + AccZ * 7) / 8;
}
 
//############################################################################
//Hauptprogramm
218,13 → 285,14
int main (void)
//############################################################################
{
DDRC = 0x08;
PORTC = 0x08;
DDRD = 0xf4;
PORTD = 0xA0;
char j3_state = 0, debounce = 0;
DDRC = 0x00;
PORTC = 0x0c;
DDRD = 0xfe;
PORTD = 0xA8;
DDRB = 0x04;
PORTB = 0x35;
LED_ON;
UART_Init();
240,7 → 308,9
Ymax = eeprom_read_word(&eeYmax);
Zmin = eeprom_read_word(&eeZmin);
Zmax = eeprom_read_word(&eeZmax);
 
OffsetAccN = eeprom_read_word(&eeNoffset);
OffsetAccR = eeprom_read_word(&eeRoffset);
OffsetAccZ = eeprom_read_word(&eeZoffset);
VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
VersionInfo.Nebenversion = VERSION_NEBENVERSION;
VersionInfo.PCKompatibel = 7;
248,14 → 318,29
ExternData.Orientation = 0;
ExternData.CalState = 0;
I2C_WriteCal.CalByte = 0;
ExternCalState = 0;
Delay_ms(10);
RawAccR = MessAD(2);
RawAccN = MessAD(3);
if(RawAccR > 1000 && RawAccN > 1000) AccPresent = 0; else AccPresent = 1;
if(AccPresent)
{
uart_putchar ('A');uart_putchar ('C');uart_putchar ('C');uart_putchar ('\n');
}
PORTC &= ~0x0c;
Delay_ms(100);
if(!PIN_J3) { ExternCalState++; j3_state = 1;}
RawAccR = MessAD(2);
RawAccN = MessAD(3);
RawAccZ = MessAD(6);
while (1)
{
FLIP_LOW;
Delay_ms(2);
RawMagnet1a = MessAD(0);
RawMagnet2a = -MessAD(1);
RawMagnet3a = MessAD(7);
RawMagnet1b = MessAD(0);
RawMagnet2b = -MessAD(1);
RawMagnet3b = MessAD(7);
AccMeasurement();
Delay_ms(1);
 
FLIP_HIGH;
263,13 → 348,21
RawMagnet1b = MessAD(0);
RawMagnet2b = -MessAD(1);
RawMagnet3b = MessAD(7);
AccMeasurement();
Delay_ms(1);
 
CalcFields();
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
if(ExternData.CalState || I2C_WriteCal.CalByte || ExternCalState) Calibrate();
else CalcHeading();
BearbeiteRxDaten();
 
if(ExternCalState)
if(!debounce--)
{
if(!PIN_J3) { if(!j3_state) ExternCalState++; j3_state = 1;} else j3_state = 0;
debounce = 20;
}
 
if(PC_Connected)
{
DDRD |= 0x02; // TXD-Portpin
/trunk/main.h
55,12 → 55,16
#define LED_OFF PORTD &= ~0x80
#define FLIP_HIGH PORTD |= 0x60
#define FLIP_LOW PORTD &= ~0x60
#define PIN_J3 (PIND & 0x08)
 
extern unsigned int PwmHeading;
extern unsigned int PC_Connected;
extern unsigned int Heading;
extern signed int MagnetN,MagnetR,MagnetZ;
 
extern signed int RawAccN, RawAccR, RawAccZ;
extern signed int AccN, AccR, AccZ;
extern signed int OffsetAccN, OffsetAccR, OffsetAccZ;
extern unsigned char AngleSource;
extern uint16_t eeXmin EEMEM;
extern uint16_t eeXmax EEMEM;
extern uint16_t eeYmin EEMEM;
/trunk/makefile
3,7 → 3,7
MCU = atmega168
#-------------------------------------------------------------------
HAUPT_VERSION = 0
NEBEN_VERSION = 14
NEBEN_VERSION = 15
#-------------------------------------------------------------------
 
# Output format. (can be srec, ihex, binary)
/trunk/twislave.c
71,9 → 71,9
struct str_I2C_EEPROM I2C_ReadEEPROM, I2C_WriteEEPROM;
struct str_I2C_Version I2C_Version;
struct str_I2C_WriteCal I2C_WriteCal;
struct str_I2C_Acc I2C_Acc;
 
 
 
//############################################################################
//I2C (TWI) Interface Init
void InitIC2_Slave(void)
144,6 → 144,14
I2C_Mag.MagZ = MagnetZ;
break;
case I2C_CMD_READ_ACC:
I2C_TxBuffer = (unsigned char *)&I2C_Acc;
I2C_TxBufferSize = sizeof(I2C_Acc);
I2C_RxBufferSize = 0;
I2C_Acc.AccN = RawAccN;
I2C_Acc.AccR = RawAccR;
I2C_Acc.AccZ = RawAccZ;
break;
case I2C_CMD_READ_HEADING:
I2C_TxBuffer = (unsigned char *)&I2C_Heading;
I2C_TxBufferSize = sizeof(I2C_Heading);
153,7 → 161,19
I2C_Heading.Heading = Heading;
ExternData.Winkel[NICK] = I2C_WriteNickRoll.Nick;
ExternData.Winkel[ROLL] = I2C_WriteNickRoll.Roll;
AngleSource = 1;
break;
case I2C_CMD_READ_HEADING_INTERNAL:
I2C_TxBuffer = (unsigned char *)&I2C_Heading;
I2C_TxBufferSize = sizeof(I2C_Heading);
I2C_RxBufferSize = 0;
I2C_Heading.Heading = Heading;
ExternData.Winkel[NICK] = 0;
ExternData.Winkel[ROLL] = 0;
AngleSource = 0;
break;
 
}
}
else
/trunk/twislave.h
42,12 → 42,23
unsigned char Dummy2;
} ;
 
#define I2C_CMD_READ_HEADING 0x05
 
struct str_I2C_Heading
{
unsigned int Heading;
} ;
 
#define I2C_CMD_READ_ACC 0x06
struct str_I2C_Acc
{
int AccN;
int AccR;
int AccZ;
};
 
#define I2C_CMD_READ_HEADING_INTERNAL 0x07
 
extern unsigned char Tx_Idx, Rx_Idx, I2C_Direction;
 
extern struct str_I2C_Heading I2C_Heading;
56,6 → 67,7
extern struct str_I2C_EEPROM I2C_ReadEEPROM, I2C_WriteEEPROM;
extern struct str_I2C_Version I2C_Version;
extern struct str_I2C_WriteCal I2C_WriteCal;
extern struct str_I2C_Acc I2C_Acc;
 
 
extern void InitIC2_Slave (void);
/trunk/uart.c
91,8 → 91,8
"Raw N ",
"Raw R ",
"Raw Z ", //5
"Lage N ",
"Lage R ",
"Angle N ",
"Angle R ",
"Xmin ",
"Xmax ",
"Ymin ", //10
99,19 → 99,19
"Ymax ",
"Zmin ",
"ZMax ",
"Calstate ",
"Kompass ", //15
"Compass ",
"Calstate ", //15
"User0 ",
"User1 ",
"Analog18 ",
"Analog19 ",
"Analog20 ", //20
"Analog21 ",
"Analog22 ",
"Analog23 ",
"Analog24 ",
"Analog25 ", //25
"Analog26 ",
"AccN ",
"AccR ",
"AccZ ", //20
"RawAccN ",
"RawAccR ",
"RawAccZ ",
"OffsetAccN ",
"OffsetAccR ", //25
"OffsetAccZ ",
"Analog27 ",
"Analog28 ",
"Analog29 ",
358,12 → 358,13
Decode64((unsigned char *) &ExternData,sizeof(ExternData),3,AnzahlEmpfangsBytes);
DebugOut.Analog[15]++;
KompassAntwort = 1;
AngleSource = 2;
break;
case 'c':
case 'b':
Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
ExternData.Winkel[0] = ExternControl.Par1;
ExternData.Winkel[1] = ExternControl.Par2;
// ExternData.Winkel[0] = ExternControl.Par1;
// ExternData.Winkel[1] = ExternControl.Par2;
break;
case 'v': // Version-Anforderung und Ausbaustufe
GetVersionAnforderung = 1;