Rev 2042 | Rev 2045 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2042 | Rev 2044 | ||
|---|---|---|---|
| Line 89... | Line 89... | ||
| 89 | } |
89 | } |
| 90 | return 0; |
90 | return 0; |
| 91 | } |
91 | } |
| Line 92... | Line 92... | ||
| 92 | 92 | ||
| 93 | // checks nick and roll sticks for manual control |
93 | // checks nick and roll sticks for manual control |
| 94 | uint8_t GPS_isManuallyControlled(int16_t* naviSticks) { |
94 | uint8_t GPS_isManuallyControlled(int16_t* sticks) { |
| 95 | if (naviSticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
95 | if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
| 96 | && naviSticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
96 | && sticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
| 97 | return 0; |
97 | return 0; |
| 98 | else |
98 | else |
| 99 | return 1; |
99 | return 1; |
| Line 100... | Line 100... | ||
| 100 | } |
100 | } |
| 101 | 101 | ||
| 102 | // set given position to current gps position |
- | |
| 103 | uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) { |
102 | // set given position to current gps position |
| 104 | uint8_t retval = 0; |
103 | uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) { |
| Line 105... | Line 104... | ||
| 105 | if (pGPSPos == NULL) |
104 | if (pGPSPos == NULL) |
| 106 | return (retval); // bad pointer |
105 | return 0; // bad pointer |
| 107 | 106 | ||
| 108 | if (GPS_isSignalOK()) { // is GPS signal condition is fine |
107 | if (GPS_isSignalOK()) { // is GPS signal condition is fine |
| 109 | pGPSPos->longitude = GPSInfo.longitude; |
108 | pGPSPos->longitude = GPSInfo.longitude; |
| 110 | pGPSPos->latitude = GPSInfo.latitude; |
109 | pGPSPos->latitude = GPSInfo.latitude; |
| 111 | pGPSPos->altitude = GPSInfo.altitude; |
110 | pGPSPos->altitude = GPSInfo.altitude; |
| 112 | pGPSPos->status = NEWDATA; |
111 | pGPSPos->status = NEWDATA; |
| 113 | retval = 1; |
112 | return 1; |
| 114 | } else { // bad GPS signal condition |
113 | } else { // bad GPS signal condition |
| 115 | pGPSPos->status = INVALID; |
- | |
| 116 | retval = 0; |
114 | pGPSPos->status = INVALID; |
| Line 117... | Line 115... | ||
| 117 | } |
115 | return 0; |
| 118 | return (retval); |
116 | } |
| 119 | } |
- | |
| 120 | 117 | } |
|
| 121 | // clear position |
118 | |
| 122 | uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) { |
119 | // clear position |
| 123 | uint8_t retval = 0; |
120 | uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) { |
| 124 | if (pGPSPos == NULL) |
121 | if (pGPSPos == NULL) |
| 125 | return retval; // bad pointer |
122 | return 0; // bad pointer |
| 126 | else { |
123 | else { |
| 127 | pGPSPos->longitude = 0; |
- | |
| 128 | pGPSPos->latitude = 0; |
124 | pGPSPos->longitude = 0; |
| 129 | pGPSPos->altitude = 0; |
125 | pGPSPos->latitude = 0; |
| 130 | pGPSPos->status = INVALID; |
126 | pGPSPos->altitude = 0; |
| Line 131... | Line 127... | ||
| 131 | retval = 1; |
127 | pGPSPos->status = INVALID; |
| 132 | } |
128 | } |
| 133 | return (retval); |
129 | return 1; |
| 134 | } |
130 | } |
| 135 | 131 | ||
| 136 | // calculates the GPS control stick values from the deviation to target position |
132 | // calculates the GPS control stick values from the deviation to target position |
| 137 | // if the pointer to the target positin is NULL or is the target position invalid |
133 | // if the pointer to the target positin is NULL or is the target position invalid |
| 138 | // then the P part of the controller is deactivated. |
134 | // then the P part of the controller is deactivated. |
| 139 | void GPS_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) { |
- | |
| 140 | static int32_t PID_Nick, PID_Roll; |
135 | void GPS_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) { |
| 141 | int32_t coscompass, sincompass; |
136 | static int32_t PID_Nick, PID_Roll; |
| 142 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
137 | int32_t coscompass, sincompass; |
| 143 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, |
138 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| Line 144... | Line 139... | ||
| 144 | I_East = 0; |
139 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
| 145 | int32_t PID_North = 0, PID_East = 0; |
140 | int32_t PID_North = 0, PID_East = 0; |
| 146 | static int32_t cos_target_latitude = 1; |
141 | static int32_t cos_target_latitude = 1; |
| 147 | static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
- | |
| 148 | static GPS_Pos_t *pLastTargetPos = 0; |
142 | static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
| 149 | - | ||
| 150 | // if GPS data and Compass are ok |
143 | static GPS_Pos_t *pLastTargetPos = 0; |
| 151 | if (GPS_isSignalOK() && (magneticHeading >= 0)) { |
144 | |
| 152 | if (pTargetPos != NULL) // if there is a target position |
145 | // if GPS data and Compass are ok |
| 153 | { |
146 | if (GPS_isSignalOK() && (magneticHeading >= 0)) { |
| 154 | if (pTargetPos->status != INVALID) // and the position data are valid |
147 | if (pTargetPos != NULL) { // if there is a target position |
| 155 | { |
148 | if (pTargetPos->status != INVALID) { // and the position data are valid |
| 156 | // if the target data are updated or the target pointer has changed |
149 | // if the target data are updated or the target pointer has changed |
| 157 | if ((pTargetPos->status != PROCESSED) |
150 | if ((pTargetPos->status != PROCESSED) |
| 158 | || (pTargetPos != pLastTargetPos)) { |
151 | || (pTargetPos != pLastTargetPos)) { |
| 159 | // reset error integral |
152 | // reset error integral |
| 160 | GPSPosDevIntegral_North = 0; |
153 | GPSPosDevIntegral_North = 0; |
| 161 | GPSPosDevIntegral_East = 0; |
154 | GPSPosDevIntegral_East = 0; |
| 162 | // recalculate latitude projection |
155 | // recalculate latitude projection |
| Line 234... | Line 227... | ||
| 234 | // copter should fly to west (positive roll). |
227 | // copter should fly to west (positive roll). |
| 235 | // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
228 | // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
| 236 | // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
229 | // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
| 237 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
230 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| Line 238... | Line 231... | ||
| 238 | 231 | ||
| 239 | coscompass = int_cos(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
232 | coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| - | 233 | sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
|
| 240 | sincompass = int_sin(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
234 | |
| 241 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
235 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
| Line 242... | Line 236... | ||
| 242 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
236 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
| 243 | 237 | ||
| Line 259... | Line 253... | ||
| 259 | 253 | ||
| Line 260... | Line 254... | ||
| 260 | GPS_updateFlightMode(); |
254 | GPS_updateFlightMode(); |
| 261 | 255 | ||
| - | 256 | // store home position if start of flight flag is set |
|
| 262 | // store home position if start of flight flag is set |
257 | if (MKFlags & MKFLAG_CALIBRATE) { |
| 263 | if (MKFlags & MKFLAG_CALIBRATE) { |
258 | MKFlags &= ~(MKFLAG_CALIBRATE); |
| 264 | if (GPS_setCurrPosition(&homePosition)) |
259 | if (GPS_setCurrPosition(&homePosition)) |
| Line 265... | Line 260... | ||
| 265 | beep(700); |
260 | beep(500); |
| 266 | } |
261 | } |
| 267 | 262 | ||
| Line 358... | Line 353... | ||
| 358 | } |
353 | } |
| 359 | // set current data as processed to avoid further calculations on the same gps data |
354 | // set current data as processed to avoid further calculations on the same gps data |
| 360 | GPSInfo.status = PROCESSED; |
355 | GPSInfo.status = PROCESSED; |
| 361 | break; |
356 | break; |
| 362 | } // eof GPSInfo.status |
357 | } // eof GPSInfo.status |
| - | 358 | ||
| - | 359 | debugOut.analog[11] = GPSInfo.satnum; |
|
| 363 | } |
360 | } |
| Line -... | Line 361... | ||
| - | 361 | ||