Rev 937 | Rev 939 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 937 | Rev 938 | ||
---|---|---|---|
Line 2... | Line 2... | ||
2 | #include <stdlib.h> |
2 | #include <stdlib.h> |
3 | #include "fc.h" |
3 | #include "fc.h" |
4 | #include "ubx.h" |
4 | #include "ubx.h" |
5 | #include "mymath.h" |
5 | #include "mymath.h" |
6 | #include "timer0.h" |
6 | #include "timer0.h" |
7 | //#include "uart.h" |
7 | #include "uart.h" |
8 | #include "rc.h" |
8 | #include "rc.h" |
9 | #include "eeprom.h" |
9 | #include "eeprom.h" |
Line 10... | Line 10... | ||
10 | 10 | ||
11 | typedef enum |
11 | typedef enum |
Line 14... | Line 14... | ||
14 | GPS_FLIGHT_MODE_FREE, |
14 | GPS_FLIGHT_MODE_FREE, |
15 | GPS_FLIGHT_MODE_AID, |
15 | GPS_FLIGHT_MODE_AID, |
16 | GPS_FLIGHT_MODE_HOME, |
16 | GPS_FLIGHT_MODE_HOME, |
17 | } FlightMode_t; |
17 | } FlightMode_t; |
Line 18... | Line 18... | ||
18 | 18 | ||
19 | #define GPS_STICK_LIMIT 200 // limit of gps stick control to avoid critical flight attitudes |
19 | #define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
20 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
20 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
Line 21... | Line 21... | ||
21 | #define GPS_P_LIMIT 100 |
21 | #define GPS_P_LIMIT 250 |
22 | 22 | ||
23 | 23 | ||
Line 147... | Line 147... | ||
147 | // calculates the GPS control stick values from the deviation to target position |
147 | // calculates the GPS control stick values from the deviation to target position |
148 | // if the pointer to the target positin is NULL or is the target position invalid |
148 | // if the pointer to the target positin is NULL or is the target position invalid |
149 | // then the P part of the controller is deactivated. |
149 | // then the P part of the controller is deactivated. |
150 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
150 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
151 | { |
151 | { |
152 | int32_t PID_Nick, PID_Roll; |
152 | static int32_t PID_Nick, PID_Roll; |
153 | int32_t coscompass, sincompass; |
153 | int32_t coscompass, sincompass; |
154 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
154 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
155 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
155 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
156 | int32_t PID_North = 0, PID_East = 0; |
156 | int32_t PID_North = 0, PID_East = 0; |
157 | static int32_t cos_target_latitude = 1; |
157 | static int32_t cos_target_latitude = 1; |
Line 204... | Line 204... | ||
204 | // reset error integral |
204 | // reset error integral |
205 | GPSPosDevIntegral_North = 0; |
205 | GPSPosDevIntegral_North = 0; |
206 | GPSPosDevIntegral_East = 0; |
206 | GPSPosDevIntegral_East = 0; |
207 | } |
207 | } |
Line 208... | Line 208... | ||
208 | 208 | ||
Line 209... | Line 209... | ||
209 | //Calculate PID-components of the controller (negative sign for compensation) |
209 | //Calculate PID-components of the controller |
210 | 210 | ||
211 | // P-Part |
211 | // P-Part |
Line 212... | Line 212... | ||
212 | P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
212 | P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
213 | P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
213 | P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
214 | 214 | ||
Line 215... | Line 215... | ||
215 | // I-Part |
215 | // I-Part |
216 | I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
216 | I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
217 | I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
217 | I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
Line 232... | Line 232... | ||
232 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
232 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
233 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
233 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
234 | } |
234 | } |
Line 235... | Line 235... | ||
235 | 235 | ||
236 | // D-Part |
236 | // D-Part |
237 | D_North = -((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
237 | D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
Line 238... | Line 238... | ||
238 | D_East = -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
238 | D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
239 | 239 | ||
240 | // combine PI- and D-Part |
240 | // combine PI- and D-Part |
Line 259... | Line 259... | ||
259 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
259 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
260 | // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
260 | // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
Line 261... | Line 261... | ||
261 | 261 | ||
262 | coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
262 | coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
263 | sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
263 | sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
264 | PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
264 | PID_Nick = (PID_Nick + (coscompass * PID_North + sincompass * PID_East) / 8192)/2; |
- | 265 | PID_Roll = (PID_Roll + (sincompass * PID_North - coscompass * PID_East) / 8192)/2; |
|
Line 265... | Line 266... | ||
265 | PID_Nick = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
266 | |
266 | 267 | ||
Line 267... | Line 268... | ||
267 | // limit resulting GPS control vector |
268 | // limit resulting GPS control vector |
Line 283... | Line 284... | ||
283 | 284 | ||
284 | 285 | ||
285 | void GPS_Main(void) |
286 | void GPS_Main(void) |
286 | { |
287 | { |
Line 287... | Line 288... | ||
287 | static uint8_t GPS_P_Delay = 0; |
288 | static uint8_t GPS_P_Delay = 0; |
Line 288... | Line 289... | ||
288 | uint16_t beep_rythm = 0; |
289 | static uint16_t beep_rythm = 0; |
289 | 290 | ||
290 | GPS_UpdateParameter(); |
291 | GPS_UpdateParameter(); |
291 | 292 | ||
292 | // store home position if start of flight flag is set |
293 | // store home position if start of flight flag is set |
Line 293... | Line 294... | ||
293 | if(MKFlags & MKFLAG_CALIBRATE) |
294 | if(MKFlags & MKFLAG_CALIBRATE) |
294 | { |
295 | { |
295 | GPS_SetCurrPosition(&HomePosition); |
296 | if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700; |