Rev 19 | Rev 21 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 19 | Rev 20 | ||
|---|---|---|---|
| Line 104... | Line 104... | ||
| 104 | MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
104 | MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
| 105 | MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range); |
105 | MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range); |
| 106 | MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range); |
106 | MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range); |
| 107 | } |
107 | } |
| Line -... | Line 108... | ||
| - | 108 | ||
| 108 | 109 | ||
| 109 | void CalcHeading(void) |
110 | void CalcHeading(void) |
| 110 | { |
111 | { |
| 111 | double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
112 | double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
| Line 112... | Line -... | ||
| 112 | int16_t heading = -1; |
- | |
| 113 | - | ||
| 114 | // calculate nick and roll angle i rad |
- | |
| Line 115... | Line 113... | ||
| 115 | nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800); |
113 | int16_t heading = -1; |
| 116 | roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800); |
114 | |
| 117 | 115 | ||
| Line 124... | Line 122... | ||
| 124 | Cx = MagnetX; |
122 | Cx = MagnetX; |
| 125 | Cy = -MagnetY; |
123 | Cy = -MagnetY; |
| 126 | Cz = MagnetZ; |
124 | Cz = MagnetZ; |
| 127 | } |
125 | } |
| Line 128... | Line 126... | ||
| 128 | 126 | ||
| 129 | Hx = Cx * (double)cos(nick_rad) + |
127 | // calculate nick and roll angle in rad |
| 130 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
128 | nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0); |
| 131 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
- | |
| - | 129 | roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0); |
|
| 132 | 130 | // calculate attitude correction |
|
| 133 | Hy = Cy * (double)cos(roll_rad) + |
131 | Hx = Cx * (double)cos(nick_rad) - Cz * (double)sin(nick_rad); |
| 134 | Cz * (double)sin(roll_rad); |
- | |
| 135 | 132 | Hy = Cy * (double)cos(roll_rad) + Cz * (double)sin(roll_rad); |
|
| 136 | 133 | ||
| 137 | if(Hx == 0 && Hy < 0) heading = 90; |
134 | // calculate Heading |
| 138 | else if(Hx == 0 && Hy > 0) heading = 270; |
135 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
| - | 136 | // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
|
| 139 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI; |
137 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
| 140 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI; |
138 | if (heading < 0) heading = -heading; |
| Line 141... | Line 139... | ||
| 141 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI; |
139 | else heading = 360 - heading; |
| 142 | - | ||
| - | 140 | ||
| 143 | if(abs(heading) < 361) Heading = heading; |
141 | if(abs(heading) < 361) Heading = heading; |
| Line 144... | Line 142... | ||
| 144 | 142 | else (Heading = -1); |
|
| 145 | } |
143 | } |