Rev 1 | Rev 5 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1 | Rev 3 | ||
---|---|---|---|
1 | signed int OffsetN, OffsetR, OffsetZ; |
1 | signed int OffsetN, OffsetR, OffsetZ; |
2 | /* |
2 | /* |
3 | signed int CalTabelleHorizontalN[4] = {67,-450,77,-411}; // Vier Messwerte 90° Horizontal gedreht |
3 | signed int CalTabelleHorizontalN[4] = {67,-450,77,-411}; // Vier Messwerte 90° Horizontal gedreht |
4 | signed int CalTabelleHorizontalR[4] = {-310,175,183,-333}; |
4 | signed int CalTabelleHorizontalR[4] = {-310,175,183,-333}; |
5 | signed int CalTabelleHorizontalZ[4] = {-100,-390,-400,100}; // Messwinkel des Z-Sensors auch horizontal gemessen |
5 | signed int CalTabelleHorizontalZ[4] = {-100,-390,-400,100}; // Messwinkel des Z-Sensors auch horizontal gemessen |
6 | */ |
6 | */ |
7 | signed int CalTabelleHorizontalN[4] = { 75,-180,-440,-166}; // Vier Messwerte 90° Horizontal gedreht |
7 | signed int CalTabelleHorizontalN[4] = { 75,-180,-440,-166}; // Vier Messwerte 90° Horizontal gedreht |
8 | signed int CalTabelleHorizontalR[4] = { -5,-230, 66, 246}; |
8 | signed int CalTabelleHorizontalR[4] = { -5,-230, 66, 246}; |
9 | signed int CalTabelleHorizontalZ[4] = {-290,-280, 44, 33}; // Messwinkel des Z-Sensors auch horizontal gemessen |
9 | signed int CalTabelleHorizontalZ[4] = {-290,-280, 44, 33}; // Messwinkel des Z-Sensors auch horizontal gemessen |
10 | 10 | ||
11 | signed int RawMagnet1a,RawMagnet1b; // reine AD-Messung |
11 | signed int RawMagnet1a,RawMagnet1b; // reine AD-Messung |
12 | signed int RawMagnet2a,RawMagnet2b; |
12 | signed int RawMagnet2a,RawMagnet2b; |
13 | signed int RawMagnet3a,RawMagnet3b; |
13 | signed int RawMagnet3a,RawMagnet3b; |
14 | signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur |
14 | signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur |
15 | signed int MagnetN,MagnetR,MagnetZ; // Kalibrierte Messerte |
15 | signed int MagnetN,MagnetR,MagnetZ; // Kalibrierte Messerte |
16 | unsigned int PwmHeading = 0; |
16 | unsigned int PwmHeading = 0; |
17 | unsigned char PC_Connected = 0; |
17 | unsigned char PC_Connected = 0; |
- | 18 | int Heading; |
|
18 | #include "main.h" |
19 | #include "main.h" |
19 | 20 | ||
20 | 21 | ||
21 | //############################################################################ |
22 | //############################################################################ |
22 | // |
23 | // |
23 | void Wait(unsigned char dauer) |
24 | void Wait(unsigned char dauer) |
24 | //############################################################################ |
25 | //############################################################################ |
25 | { |
26 | { |
26 | dauer = (unsigned char)TCNT0 + dauer; |
27 | dauer = (unsigned char)TCNT0 + dauer; |
27 | while((TCNT0 - dauer) & 0x80); |
28 | while((TCNT0 - dauer) & 0x80); |
28 | } |
29 | } |
29 | 30 | ||
30 | void CalcFields(void) |
31 | void CalcFields(void) |
31 | { |
32 | { |
32 | UncalMagnetN = (2 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 3; |
33 | UncalMagnetN = (2 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 3; |
33 | UncalMagnetR = (2 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 3; |
34 | UncalMagnetR = (2 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 3; |
34 | UncalMagnetZ = (2 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 3; |
35 | UncalMagnetZ = (2 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 3; |
35 | MagnetN = UncalMagnetN - OffsetN; |
36 | MagnetN = UncalMagnetN - OffsetN; |
36 | MagnetR = UncalMagnetR - OffsetR; |
37 | MagnetR = UncalMagnetR - OffsetR; |
37 | MagnetZ = UncalMagnetZ - OffsetZ; |
38 | MagnetZ = UncalMagnetZ - OffsetZ; |
38 | } |
39 | } |
39 | 40 | ||
40 | //------------------------------------------------------ |
41 | //------------------------------------------------------ |
41 | void CalcHeading(void) |
42 | void CalcHeading(void) |
42 | { |
43 | { |
43 | double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
44 | double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
44 | float nick, roll, XEx, XEy, YEy, YEz, YE, XE, XrawCal, YrawCal, ZrawCal, XEz; |
45 | float nick, roll, XEx, XEy, YEy, YEz, YE, XE, XrawCal, YrawCal, ZrawCal, XEz; |
45 | int heading, azimuthgrad; |
46 | int heading, azimuthgrad; |
46 | 47 | ||
47 | nick_rad = ((double)WinkelOut.Winkel[0]) * M_PI / (double)(180); |
48 | nick_rad = ((double)WinkelOut.Winkel[0]) * M_PI / (double)(180); |
48 | roll_rad = ((double)WinkelOut.Winkel[1]) * M_PI / (double)(180); |
49 | roll_rad = ((double)WinkelOut.Winkel[1]) * M_PI / (double)(180); |
49 | //roll_rad = 0; nick_rad = 0; |
50 | //roll_rad = 0; nick_rad = 0; |
50 | 51 | ||
51 | /* |
52 | /* |
52 | Cx = (double) (MicroMag.Axis[Y_AXIS] + CY_OFFSET) * 0.707f - (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f; |
53 | Cx = (double) (MicroMag.Axis[Y_AXIS] + CY_OFFSET) * 0.707f - (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f; |
53 | Cy = ((double) (MicroMag.Axis[Y_AXIS]+ CY_OFFSET) * 0.707f + (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f); |
54 | Cy = ((double) (MicroMag.Axis[Y_AXIS]+ CY_OFFSET) * 0.707f + (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f); |
54 | Cz = -MicroMag.Axis[Z_AXIS] + CZ_OFFSET; |
55 | Cz = -MicroMag.Axis[Z_AXIS] + CZ_OFFSET; |
55 | */ |
56 | */ |
56 | Cx = MagnetN; |
57 | Cx = MagnetN; |
57 | Cy = MagnetR; |
58 | Cy = MagnetR; |
58 | Cz = -MagnetZ; |
59 | Cz = -MagnetZ; |
59 | 60 | ||
60 | Hx = Cx * (double)cos(nick_rad) + |
61 | Hx = Cx * (double)cos(nick_rad) + |
61 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
62 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
62 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
63 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
63 | 64 | ||
64 | Hy = Cy * (double)cos(roll_rad) + |
65 | Hy = Cy * (double)cos(roll_rad) + |
65 | Cz * (double)sin(roll_rad); |
66 | Cz * (double)sin(roll_rad); |
66 | 67 | ||
67 | 68 | ||
68 | if(Hx == 0 && Hy < 0) heading = 90; |
69 | if(Hx == 0 && Hy < 0) heading = 90; |
69 | else if(Hx == 0 && Hy > 0) heading = 270; |
70 | else if(Hx == 0 && Hy > 0) heading = 270; |
70 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180 / M_PI); |
71 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180 / M_PI); |
71 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180 / M_PI); |
72 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180 / M_PI); |
72 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180 / M_PI); |
73 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180 / M_PI); |
73 | 74 | ||
74 | // DebugOut.Analog[14] = heading; |
75 | // DebugOut.Analog[14] = heading; |
75 | // if (FromFlightCtrl.IntegralNick > 0) heading = heading + FromFlightCtrl.IntegralNick/60; |
76 | // if (FromFlightCtrl.IntegralNick > 0) heading = heading + FromFlightCtrl.IntegralNick/60; |
76 | 77 | ||
77 | if(heading < 361) DebugOut.Analog[14] = heading; |
78 | if(heading < 361) DebugOut.Analog[14] = heading; |
78 | AnFlightCtrl.Heading = heading; |
79 | Heading = heading; |
79 | PwmHeading = heading + 10; |
80 | PwmHeading = heading + 10; |
80 | // MicroMag.Heading = heading; |
81 | // MicroMag.Heading = heading; |
81 | // DebugOut.Analog[14] = heading; |
82 | // DebugOut.Analog[14] = heading; |
82 | } |
83 | } |
83 | 84 | ||
84 | 85 | ||
85 | 86 | ||
86 | //############################################################################ |
87 | //############################################################################ |
87 | //Hauptprogramm |
88 | //Hauptprogramm |
88 | int main (void) |
89 | int main (void) |
89 | //############################################################################ |
90 | //############################################################################ |
90 | { |
91 | { |
91 | DDRC = 0x08; |
92 | DDRC = 0x08; |
92 | PORTC = 0x08; |
93 | PORTC = 0x08; |
93 | DDRD = 0xf4; |
94 | DDRD = 0xf4; |
94 | PORTD = 0xA0; |
95 | PORTD = 0xA0; |
95 | DDRB = 0x04; |
96 | DDRB = 0x04; |
96 | PORTB = 0x35; |
97 | PORTB = 0x35; |
97 | 98 | ||
98 | LED_ON; |
99 | LED_ON; |
99 | 100 | ||
100 | UART_Init(); |
101 | UART_Init(); |
101 | Timer0_Init(); |
102 | Timer0_Init(); |
102 | ADC_Init(); |
103 | ADC_Init(); |
- | 104 | InitIC2_Slave(); |
|
103 | sei();//Globale Interrupts Einschalten |
105 | sei();//Globale Interrupts Einschalten |
104 | Debug_Timer = SetDelay(100); // Sendeintervall |
106 | Debug_Timer = SetDelay(100); // Sendeintervall |
105 | InitIC2_Slave(0x50); |
- | |
- | 107 | ||
106 | 108 | ||
107 | OffsetN = (CalTabelleHorizontalN[0] + CalTabelleHorizontalN[1] + CalTabelleHorizontalN[2] + CalTabelleHorizontalN[3]) / 4; |
109 | OffsetN = (CalTabelleHorizontalN[0] + CalTabelleHorizontalN[1] + CalTabelleHorizontalN[2] + CalTabelleHorizontalN[3]) / 4; |
108 | OffsetR = (CalTabelleHorizontalR[0] + CalTabelleHorizontalR[1] + CalTabelleHorizontalR[2] + CalTabelleHorizontalR[3]) / 4; |
110 | OffsetR = (CalTabelleHorizontalR[0] + CalTabelleHorizontalR[1] + CalTabelleHorizontalR[2] + CalTabelleHorizontalR[3]) / 4; |
109 | OffsetZ = (CalTabelleHorizontalZ[0] + CalTabelleHorizontalZ[1] + CalTabelleHorizontalZ[2] + CalTabelleHorizontalZ[3]) / 4; |
111 | OffsetZ = (CalTabelleHorizontalZ[0] + CalTabelleHorizontalZ[1] + CalTabelleHorizontalZ[2] + CalTabelleHorizontalZ[3]) / 4; |
110 | 112 | ||
111 | VersionInfo.Hauptversion = 0; |
113 | VersionInfo.Hauptversion = VERSION_HAUPTVERSION; |
112 | VersionInfo.Nebenversion = 3; |
114 | VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
113 | VersionInfo.PCKompatibel = 7; |
115 | VersionInfo.PCKompatibel = 7; |
114 | 116 | ||
115 | 117 | ||
116 | while (1) |
118 | while (1) |
117 | { |
119 | { |
118 | LED_ON; |
120 | LED_ON; |
119 | FLIP_LOW; |
121 | FLIP_LOW; |
120 | Delay_ms(2); |
122 | Delay_ms(2); |
121 | RawMagnet1a = MessAD(0); |
123 | RawMagnet1a = MessAD(0); |
122 | RawMagnet2a = MessAD(1); |
124 | RawMagnet2a = MessAD(1); |
123 | RawMagnet3a = MessAD(7); |
125 | RawMagnet3a = MessAD(7); |
124 | Delay_ms(1); |
126 | Delay_ms(1); |
125 | LED_OFF; |
127 | LED_OFF; |
126 | FLIP_HIGH; |
128 | FLIP_HIGH; |
127 | Delay_ms(2); |
129 | Delay_ms(2); |
128 | RawMagnet1b = MessAD(0); |
130 | RawMagnet1b = MessAD(0); |
129 | RawMagnet2b = MessAD(1); |
131 | RawMagnet2b = MessAD(1); |
130 | RawMagnet3b = MessAD(7); |
132 | RawMagnet3b = MessAD(7); |
131 | Delay_ms(1); |
133 | Delay_ms(1); |
132 | 134 | ||
133 | CalcFields(); |
135 | CalcFields(); |
134 | DebugOut.Analog[0] = MagnetN; |
136 | DebugOut.Analog[0] = MagnetN; |
135 | DebugOut.Analog[1] = MagnetR; |
137 | DebugOut.Analog[1] = MagnetR; |
136 | DebugOut.Analog[2] = MagnetZ; |
138 | DebugOut.Analog[2] = MagnetZ; |
137 | DebugOut.Analog[3] = UncalMagnetN; |
139 | DebugOut.Analog[3] = UncalMagnetN; |
138 | DebugOut.Analog[4] = UncalMagnetR; |
140 | DebugOut.Analog[4] = UncalMagnetR; |
139 | DebugOut.Analog[5] = UncalMagnetZ; |
141 | DebugOut.Analog[5] = UncalMagnetZ; |
140 | /* |
142 | /* |
141 | DebugOut.Analog[3] = RawMagnet1a; |
143 | DebugOut.Analog[3] = RawMagnet1a; |
142 | DebugOut.Analog[4] = RawMagnet1b; |
144 | DebugOut.Analog[4] = RawMagnet1b; |
143 | DebugOut.Analog[5] = RawMagnet3a; |
145 | DebugOut.Analog[5] = RawMagnet3a; |
144 | DebugOut.Analog[6] = RawMagnet3b;*/ |
146 | DebugOut.Analog[6] = RawMagnet3b;*/ |
145 | DebugOut.Analog[6] = WinkelOut.Winkel[0]; |
147 | DebugOut.Analog[6] = WinkelOut.Winkel[0]; |
146 | DebugOut.Analog[7] = WinkelOut.Winkel[1]; |
148 | DebugOut.Analog[7] = WinkelOut.Winkel[1]; |
147 | 149 | ||
148 | CalcHeading(); |
150 | CalcHeading(); |
149 | BearbeiteRxDaten(); |
151 | BearbeiteRxDaten(); |
150 | PC_Connected = 100; |
152 | PC_Connected = 100; |
151 | if(PC_Connected) |
153 | if(PC_Connected) |
152 | { |
154 | { |
153 | PC_Connected--; |
155 | PC_Connected--; |
154 | DatenUebertragung(); |
156 | DatenUebertragung(); |
155 | DDRD |= 0x02; // TXD-Leitung |
157 | DDRD |= 0x02; // TXD-Leitung |
156 | } |
158 | } |
157 | else |
159 | else |
158 | { |
160 | { |
159 | DDRD &= ~0x02; // TXD-Leitung |
161 | DDRD &= ~0x02; // TXD-Leitung |
160 | } |
162 | } |
161 | } // while(1) - Hauptschleife |
163 | } // while(1) - Hauptschleife |
162 | } |
164 | } |
163 | 165 | ||
164 | 166 |