Rev 35 | Rev 37 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 35 | Rev 36 | ||
|---|---|---|---|
| Line 64... | Line 64... | ||
| 64 | #include "twislave.h" |
64 | #include "twislave.h" |
| 65 | #include "led.h" |
65 | #include "led.h" |
| 66 | #include "analog.h" |
66 | #include "analog.h" |
| 67 | #include "uart.h" |
67 | #include "uart.h" |
| Line -... | Line 68... | ||
| - | 68 | ||
| Line 68... | Line 69... | ||
| 68 | 69 | #define CALIBRATION_VERSION 1 |
|
| 69 | 70 | ||
| Line 70... | Line 71... | ||
| 70 | AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
71 | AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
| Line 84... | Line 85... | ||
| 84 | Scaling_t MagY; |
85 | Scaling_t MagY; |
| 85 | Scaling_t MagZ; |
86 | Scaling_t MagZ; |
| 86 | Scaling_t AccX; |
87 | Scaling_t AccX; |
| 87 | Scaling_t AccY; |
88 | Scaling_t AccY; |
| 88 | Scaling_t AccZ; |
89 | Scaling_t AccZ; |
| - | 90 | unsigned char Version; |
|
| 89 | } Calibration_t; |
91 | } Calibration_t; |
| Line 90... | Line 92... | ||
| 90 | 92 | ||
| 91 | Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
93 | Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
| Line 122... | Line 124... | ||
| 122 | if(AccPresent) |
124 | if(AccPresent) |
| 123 | { |
125 | { |
| 124 | AccX = (RawAccX - Calibration.AccX.Offset); |
126 | AccX = (RawAccX - Calibration.AccX.Offset); |
| 125 | AccY = (RawAccY - Calibration.AccY.Offset); |
127 | AccY = (RawAccY - Calibration.AccY.Offset); |
| 126 | AccZ = (Calibration.AccZ.Offset - RawAccZ); |
128 | AccZ = (Calibration.AccZ.Offset - RawAccZ); |
| 127 | - | ||
| 128 | #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL |
129 | #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL |
| 129 | // acc mode assumes orientation like FC |
130 | // acc mode assumes orientation like FC |
| 130 | if(AccX > 136) AccAttitudeNick = -800; |
131 | if(AccX > 136) AccAttitudeNick = -800; |
| 131 | else |
132 | else |
| 132 | if(AccX < -136) AccAttitudeNick = 800; |
133 | if(AccX < -136) AccAttitudeNick = 800; |
| Line 162... | Line 163... | ||
| 162 | int16_t heading = -1; |
163 | int16_t heading = -1; |
| Line 163... | Line 164... | ||
| 163 | 164 | ||
| 164 | // blink code for normal operation |
165 | // blink code for normal operation |
| 165 | if(CheckDelay(Led_Timer)) |
166 | if(CheckDelay(Led_Timer)) |
| - | 167 | { |
|
| 166 | { |
168 | if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE; |
| 167 | LED_GRN_TOGGLE; |
169 | else LED_GRN_ON; |
| 168 | Led_Timer = SetDelay(500); |
170 | Led_Timer = SetDelay(150); |
| 169 | } |
- | |
| 170 | 171 | } |
|
| 171 | switch(Orientation) |
172 | switch(Orientation) |
| 172 | { |
173 | { |
| 173 | case ORIENTATION_NC: |
174 | case ORIENTATION_NC: |
| 174 | Cx = MagX; |
175 | Cx = MagX; |
| Line 210... | Line 211... | ||
| 210 | } |
211 | } |
| Line 211... | Line 212... | ||
| 211 | 212 | ||
| 212 | nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
213 | nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
| Line 213... | Line -... | ||
| 213 | roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
- | |
| 214 | 214 | roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
|
| 215 | 215 | ||
| 216 | // calculate attitude correction |
216 | // calculate attitude correction |
| Line 217... | Line 217... | ||
| 217 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
217 | Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
| Line 226... | Line 226... | ||
| 226 | // 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. |
| 227 | if (heading < 0) heading = -heading; |
227 | if (heading < 0) heading = -heading; |
| 228 | else heading = 360 - heading; |
228 | else heading = 360 - heading; |
| Line 229... | Line 229... | ||
| 229 | 229 | ||
| 230 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 231 | /* // Alternative formula |
- | |
| 232 | Hx = Cx * (double)cos(nick_rad) + |
- | |
| 233 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
- | |
| 234 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
- | |
| 235 | - | ||
| 236 | Hy = Cy * (double)cos(roll_rad) + |
- | |
| 237 | Cz * (double)sin(roll_rad); |
- | |
| 238 | - | ||
| 239 | if(Hx == 0 && Hy < 0) heading = 90; |
- | |
| 240 | else if(Hx == 0 && Hy > 0) heading = 270; |
- | |
| 241 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI; |
- | |
| 242 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI; |
230 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 243 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI; |
- | |
| 244 | - | ||
| 245 | if(abs(heading) < 361) Heading = heading; |
- | |
| 246 | */ |
231 | if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match |
| 247 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 248 | 232 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 249 | cli(); // stop interrupts |
233 | cli(); // stop interrupts |
| 250 | if(abs(heading) < 361) Heading = heading; |
234 | if(abs(heading) < 361) Heading = heading; |
| 251 | else (Heading = -1); |
235 | else (Heading = -1); |
| Line 252... | Line 236... | ||
| 252 | sei(); // start interrupts |
236 | sei(); // start interrupts |
| Line 253... | Line -... | ||
| 253 | - | ||
| 254 | } |
237 | |
| 255 | 238 | } |
|
| 256 | 239 | ||
| 257 | void Calibrate(void) |
240 | void Calibrate(void) |
| 258 | { |
241 | { |
| Line 342... | Line 325... | ||
| 342 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
325 | Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
| 343 | Calibration.MagZ.Range = Zmax - Zmin; |
326 | Calibration.MagZ.Range = Zmax - Zmin; |
| 344 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
327 | Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
| 345 | if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
328 | if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
| 346 | { |
329 | { |
| - | 330 | Calibration.Version = CALIBRATION_VERSION; |
|
| 347 | // indicate write process by setting the led off for 2 seconds |
331 | // indicate write process by setting the led off for 2 seconds |
| 348 | LED_GRN_OFF; |
332 | LED_GRN_OFF; |
| 349 | eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
333 | eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
| 350 | Led_Timer = SetDelay(2000); |
334 | Led_Timer = SetDelay(2000); |
| 351 | // reset blinkcode |
335 | // reset blinkcode |