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 |