Rev 26 | Rev 28 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 26 | Rev 27 | ||
|---|---|---|---|
| Line 98... | Line 98... | ||
| 98 | int16_t Heading = -1; |
98 | int16_t Heading = -1; |
| Line 99... | Line 99... | ||
| 99 | 99 | ||
| 100 | 100 | ||
| 101 | void CalcFields(void) |
101 | void CalcFields(void) |
| 102 | { |
102 | { |
| 103 | UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2; |
103 | UncalMagnetX = (RawMagnet1a - RawMagnet1b) / 2; |
| Line 104... | Line 104... | ||
| 104 | UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2; |
104 | UncalMagnetY = (RawMagnet3a - RawMagnet3b) / 2; |
| 105 | UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2; |
105 | UncalMagnetZ = (RawMagnet2a - RawMagnet2b) / 2; |
| 106 | 106 | ||
| 107 | if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
107 | if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range); |
| Line 135... | Line 135... | ||
| 135 | Cy = -MagnetY; |
135 | Cy = -MagnetY; |
| 136 | Cz = MagnetZ; |
136 | Cz = MagnetZ; |
| 137 | } |
137 | } |
| Line 138... | Line 138... | ||
| 138 | 138 | ||
| 139 | // calculate nick and roll angle in rad |
139 | // calculate nick and roll angle in rad |
| 140 | nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0); |
140 | nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0); |
| 141 | roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0); |
141 | roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0); |
| 142 | // calculate attitude correction |
142 | // calculate attitude correction |
| 143 | Hx = Cx * (double)cos(nick_rad) - Cz * (double)sin(nick_rad); |
143 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
| Line 144... | Line 144... | ||
| 144 | Hy = Cy * (double)cos(roll_rad) + Cz * (double)sin(roll_rad); |
144 | Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
| 145 | 145 | ||
| 146 | // calculate Heading |
146 | // calculate Heading |
| 147 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
147 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
| Line 264... | Line 264... | ||
| 264 | DebugOut.Analog[1] = MagnetY; |
264 | DebugOut.Analog[1] = MagnetY; |
| 265 | DebugOut.Analog[2] = MagnetZ; |
265 | DebugOut.Analog[2] = MagnetZ; |
| 266 | DebugOut.Analog[3] = UncalMagnetX; |
266 | DebugOut.Analog[3] = UncalMagnetX; |
| 267 | DebugOut.Analog[4] = UncalMagnetY; |
267 | DebugOut.Analog[4] = UncalMagnetY; |
| 268 | DebugOut.Analog[5] = UncalMagnetZ; |
268 | DebugOut.Analog[5] = UncalMagnetZ; |
| 269 | DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
269 | DebugOut.Analog[6] = I2C_WriteAttitude.Nick; |
| 270 | DebugOut.Analog[7] = ExternData.Attitude[ROLL]; |
270 | DebugOut.Analog[7] = I2C_WriteAttitude.Roll; |
| 271 | DebugOut.Analog[8] = Calibration.X.Offset; |
271 | DebugOut.Analog[8] = Calibration.X.Offset; |
| 272 | DebugOut.Analog[9] = Calibration.X.Range; |
272 | DebugOut.Analog[9] = Calibration.X.Range; |
| 273 | DebugOut.Analog[10] = Calibration.Y.Offset; |
273 | DebugOut.Analog[10] = Calibration.Y.Offset; |
| 274 | DebugOut.Analog[11] = Calibration.Y.Range; |
274 | DebugOut.Analog[11] = Calibration.Y.Range; |
| 275 | DebugOut.Analog[12] = Calibration.Z.Offset; |
275 | DebugOut.Analog[12] = Calibration.Z.Offset; |