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 |