Rev 32 | Rev 34 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 32 | Rev 33 | ||
|---|---|---|---|
| Line 165... | Line 165... | ||
| 165 | if(CheckDelay(Led_Timer)) |
165 | if(CheckDelay(Led_Timer)) |
| 166 | { |
166 | { |
| 167 | LED_GRN_TOGGLE; |
167 | LED_GRN_TOGGLE; |
| 168 | Led_Timer = SetDelay(500); |
168 | Led_Timer = SetDelay(500); |
| 169 | } |
169 | } |
| 170 | //MagX = 150; |
- | |
| 171 | //MagZ = 1000; |
- | |
| Line 172... | Line 170... | ||
| 172 | 170 | ||
| 173 | switch(Orientation) |
171 | switch(Orientation) |
| 174 | { |
172 | { |
| 175 | case ORIENTATION_NC: |
173 | case ORIENTATION_NC: |
| Line 212... | Line 210... | ||
| 212 | } |
210 | } |
| Line 213... | Line 211... | ||
| 213 | 211 | ||
| 214 | nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
212 | nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
| Line 215... | Line -... | ||
| 215 | roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
- | |
| 216 | - | ||
| Line 217... | Line 213... | ||
| 217 | // nick_rad = 0; |
213 | roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
| 218 | // roll_rad = 0; |
214 | |
| 219 | 215 | ||
| Line 228... | Line 224... | ||
| 228 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
224 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
| 229 | // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
225 | // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
| 230 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
226 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
| 231 | if (heading < 0) heading = -heading; |
227 | if (heading < 0) heading = -heading; |
| 232 | else heading = 360 - heading; |
228 | else heading = 360 - heading; |
| - | 229 | ||
| 233 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
230 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 234 | /* |
231 | /* // Alternative formula |
| 235 | Hx = Cx * (double)cos(nick_rad) + |
232 | Hx = Cx * (double)cos(nick_rad) + |
| 236 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
233 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
| 237 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
234 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
| Line 238... | Line 235... | ||
| 238 | |
235 | |