Rev 733 | Rev 745 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 733 | Rev 741 | ||
|---|---|---|---|
| Line 384... | Line 384... | ||
| 384 | if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
384 | if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
| 385 | if(MotorTest[2]) Motor_Left = MotorTest[2]; |
385 | if(MotorTest[2]) Motor_Left = MotorTest[2]; |
| 386 | if(MotorTest[3]) Motor_Right = MotorTest[3]; |
386 | if(MotorTest[3]) Motor_Right = MotorTest[3]; |
| 387 | } |
387 | } |
| Line 388... | Line 388... | ||
| 388 | 388 | ||
| 389 | DebugOut.Analog[12] = Motor_Front; |
389 | //DebugOut.Analog[12] = Motor_Front; |
| 390 | DebugOut.Analog[13] = Motor_Rear; |
390 | //DebugOut.Analog[13] = Motor_Rear; |
| 391 | DebugOut.Analog[14] = Motor_Left; |
391 | //DebugOut.Analog[14] = Motor_Left; |
| Line 392... | Line 392... | ||
| 392 | DebugOut.Analog[15] = Motor_Right; |
392 | //DebugOut.Analog[15] = Motor_Right; |
| 393 | 393 | ||
| 394 | //Start I2C Interrupt Mode |
394 | //Start I2C Interrupt Mode |
| 395 | twi_state = 0; |
395 | twi_state = 0; |
| Line 1018... | Line 1018... | ||
| 1018 | if (!updCompass--) |
1018 | if (!updCompass--) |
| 1019 | { |
1019 | { |
| 1020 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
1020 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| 1021 | // get current compass heading (angule between MK head and magnetic north) |
1021 | // get current compass heading (angule between MK head and magnetic north) |
| 1022 | CompassHeading = MM3_Heading(); |
1022 | CompassHeading = MM3_Heading(); |
| - | 1023 | if (CompassHeading < 0) // no compass data available |
|
| - | 1024 | { |
|
| - | 1025 | CompassOffCourse = 0; |
|
| - | 1026 | if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem |
|
| - | 1027 | } |
|
| 1023 | // calculate OffCourse (angular deviation from heading to course) |
1028 | else // calculate OffCourse (angular deviation from heading to course) |
| 1024 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
1029 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| 1025 | - | ||
| 1026 | } |
1030 | } |
| Line 1027... | Line 1031... | ||
| 1027 | 1031 | ||
| 1028 | // reduce compass effect with increasing declination |
1032 | // reduce compass effect with increasing declination |
| 1029 | w = abs(IntegralPitch / 512); |
1033 | w = abs(IntegralPitch / 512); |
| Line 1043... | Line 1047... | ||
| 1043 | } |
1047 | } |
| 1044 | } |
1048 | } |
| 1045 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1049 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1046 | // GPS |
1050 | // GPS |
| 1047 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1051 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1048 | if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
1052 | if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding) |
| 1049 | { |
1053 | { |
| 1050 | GPS_P_Factor = FCParam.UserParam5; |
1054 | GPS_P_Factor = FCParam.UserParam5; |
| 1051 | GPS_D_Factor = FCParam.UserParam6; |
1055 | GPS_D_Factor = FCParam.UserParam6; |
| 1052 | GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data |
1056 | GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data |
| 1053 | } |
1057 | } |