Rev 2046 | Rev 2048 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2046 | Rev 2047 | ||
---|---|---|---|
Line 22... | Line 22... | ||
22 | GPS_FLIGHT_MODE_AID, |
22 | GPS_FLIGHT_MODE_AID, |
23 | GPS_FLIGHT_MODE_HOME, |
23 | GPS_FLIGHT_MODE_HOME, |
24 | } FlightMode_t; |
24 | } FlightMode_t; |
Line 25... | Line 25... | ||
25 | 25 | ||
26 | #define GPS_POSINTEGRAL_LIMIT 32000 |
26 | #define GPS_POSINTEGRAL_LIMIT 32000 |
27 | #define GPS_STICK_LIMIT 400 // limit of gps stick control to avoid critical flight attitudes |
27 | #define LOG_NAVI_STICK_GAIN 2 |
Line 28... | Line 28... | ||
28 | #define GPS_P_LIMIT 100 |
28 | #define GPS_P_LIMIT 100 |
29 | 29 | ||
30 | typedef struct { |
30 | typedef struct { |
Line 164... | Line 164... | ||
164 | // calculate position deviation from latitude and longitude differences |
164 | // calculate position deviation from latitude and longitude differences |
165 | GPSPosDev_North = (GPSInfo.latitude - pTargetPos->latitude); // to calculate real cm we would need *111/100 additionally |
165 | GPSPosDev_North = (GPSInfo.latitude - pTargetPos->latitude); // to calculate real cm we would need *111/100 additionally |
166 | GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally |
166 | GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally |
167 | // calculate latitude projection |
167 | // calculate latitude projection |
168 | GPSPosDev_East *= cos_target_latitude; |
168 | GPSPosDev_East *= cos_target_latitude; |
169 | GPSPosDev_East >>= MATH_UNIT_FACTOR_LOG; |
169 | GPSPosDev_East >>= LOG_MATH_UNIT_FACTOR; |
170 | } else { // no valid target position available |
170 | } else { // no valid target position available |
171 | // reset error |
171 | // reset error |
172 | GPSPosDev_North = 0; |
172 | GPSPosDev_North = 0; |
173 | GPSPosDev_East = 0; |
173 | GPSPosDev_East = 0; |
174 | // reset error integral |
174 | // reset error integral |
Line 231... | Line 231... | ||
231 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
231 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
Line 232... | Line 232... | ||
232 | 232 | ||
233 | coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
233 | coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
Line 234... | Line 234... | ||
234 | sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
234 | sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
235 | 235 | ||
Line 236... | Line 236... | ||
236 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2); |
236 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
237 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2); |
237 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
Line 238... | Line 238... | ||
238 | 238 | ||
239 | // limit resulting GPS control vector |
239 | // limit resulting GPS control vector |
Line 240... | Line 240... | ||
240 | navi_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
240 | navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |