| 24,7 → 24,7 |
|---|
| } FlightMode_t; |
| |
| #define GPS_POSINTEGRAL_LIMIT 32000 |
| #define GPS_STICK_LIMIT 400 // limit of gps stick control to avoid critical flight attitudes |
| #define LOG_NAVI_STICK_GAIN 2 |
| #define GPS_P_LIMIT 100 |
| |
| typedef struct { |
| 166,7 → 166,7 |
|---|
| GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East >>= MATH_UNIT_FACTOR_LOG; |
| GPSPosDev_East >>= LOG_MATH_UNIT_FACTOR; |
| } else { // no valid target position available |
| // reset error |
| GPSPosDev_North = 0; |
| 233,11 → 233,11 |
|---|
| coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2); |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2); |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| |
| // limit resulting GPS control vector |
| navi_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
| |
| debugOut.analog[27] = -PID_Nick; |
| debugOut.analog[28] = -PID_Roll; |