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 | } |