Rev 782 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 782 | Rev 792 | ||
---|---|---|---|
Line 79... | Line 79... | ||
79 | { |
79 | { |
80 | GPS_Pitch = 0; |
80 | GPS_Pitch = 0; |
81 | GPS_Roll = 0; |
81 | GPS_Roll = 0; |
82 | } |
82 | } |
Line 83... | Line 83... | ||
83 | 83 | ||
- | 84 | // calculates the GPS control stick values from the deviation to target position |
|
- | 85 | // if the pointer to the target positin is NULL or is the target position invalid |
|
84 | // calculates the GPS control sticks values from the deviation to target position |
86 | // then the P part of the controller is deactivated. |
85 | void GPS_PDController(GPS_Pos_t *TargetPos) |
87 | void GPS_PDController(GPS_Pos_t *pTargetPos) |
86 | { |
88 | { |
87 | int32_t coscompass, sincompass; |
89 | int32_t coscompass, sincompass; |
88 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
90 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
89 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
91 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
90 | int32_t PD_North = 0, PD_East = 0; |
92 | int32_t PD_North = 0, PD_East = 0; |
Line 91... | Line -... | ||
91 | static int32_t cos_target_latitude = 1; |
- | |
- | 93 | static int32_t cos_target_latitude = 1; |
|
92 | 94 | ||
93 | 95 | // if GPS data and Compass are ok |
|
94 | if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
- | |
95 | { |
- | |
96 | // calculate position deviation from latitude and longitude differences |
- | |
97 | GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
- | |
98 | GPSPosDev_East = (GPSInfo.longitude - TargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
- | |
- | 96 | if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) ) |
|
99 | // recalculate the target latitude projection only if the target data are updated |
97 | { |
100 | // to save time |
98 | |
- | 99 | if(pTargetPos != NULL) // if there is a target position |
|
- | 100 | { |
|
- | 101 | if(pTargetPos->Status != INVALID) // and the position data are valid |
|
- | 102 | { // calculate position deviation from latitude and longitude differences |
|
- | 103 | GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
|
- | 104 | GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
|
- | 105 | // recalculate the target latitude projection only if the target data are updated |
|
- | 106 | // to save time |
|
101 | if (TargetPos->Status != PROCESSED) |
107 | if (pTargetPos->Status != PROCESSED) |
102 | { |
108 | { |
- | 109 | cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
|
- | 110 | pTargetPos->Status = PROCESSED; |
|
- | 111 | } |
|
- | 112 | // calculate latitude projection |
|
- | 113 | GPSPosDev_East *= cos_target_latitude; |
|
- | 114 | GPSPosDev_East /= 8192; |
|
- | 115 | ||
- | 116 | DebugOut.Analog[12] = GPSPosDev_North; |
|
- | 117 | DebugOut.Analog[13] = GPSPosDev_East; |
|
- | 118 | //DebugOut.Analog[12] = GPSInfo.velnorth; |
|
- | 119 | //DebugOut.Analog[13] = GPSInfo.veleast; |
|
- | 120 | ||
- | 121 | //Calculate P-components of the controller (negative sign for compensation) |
|
- | 122 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
|
- | 123 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
|
- | 124 | } |
|
- | 125 | else // not valid target position available |
|
- | 126 | { |
|
- | 127 | GPSPosDev_North = 0; |
|
- | 128 | GPSPosDev_East = 0; |
|
- | 129 | } |
|
- | 130 | } |
|
- | 131 | else // not target position available |
|
- | 132 | { |
|
103 | cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L)); |
133 | GPSPosDev_North = 0; |
104 | TargetPos->Status = PROCESSED; |
- | |
105 | } |
- | |
106 | // calculate latitude projection |
- | |
107 | GPSPosDev_East *= cos_target_latitude; |
- | |
108 | GPSPosDev_East /= 8192; |
- | |
109 | - | ||
110 | DebugOut.Analog[12] = GPSPosDev_North; |
- | |
111 | DebugOut.Analog[13] = GPSPosDev_East; |
- | |
112 | - | ||
Line 113... | Line 134... | ||
113 | //DebugOut.Analog[12] = GPSInfo.velnorth; |
134 | GPSPosDev_East = 0; |
114 | //DebugOut.Analog[13] = GPSInfo.veleast; |
135 | } |
115 | - | ||
116 | //Calculate PD-components of the controller (negative sign for compensation) |
136 | |
- | 137 | //Calculate PD-components of the controller (negative sign for compensation) |
|
117 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
138 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
118 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
- | |
- | 139 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
|
- | 140 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
|
119 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
141 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
120 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
142 | // limit D-Part if position deviation is significant of the target position |
121 | 143 | // this accelerates flying to the target position |
|
122 | if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) ) |
144 | if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) ) |
123 | { |
145 | { |
124 | if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT; |
146 | if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT; |
125 | else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT; |
147 | else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT; |
126 | if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT; |
- | |
127 | else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT; |
148 | if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT; |
128 | } |
149 | else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT; |
129 | 150 | } |
|
Line 130... | Line 151... | ||
130 | // PD-controller |
151 | // PD-controller |
Line 131... | Line 152... | ||
131 | PD_North = P_North + D_North; |
152 | PD_North = P_North + D_North; |
132 | PD_East = P_East + D_East; |
153 | PD_East = P_East + D_East; |
133 | - | ||
134 | // GPS to pitch and roll settings |
154 | |
135 | 155 | // GPS to pitch and roll settings |
|
136 | //A positive pitch angle moves head downwards (flying forward). |
156 | |
137 | //A positive roll angle tilts left side downwards (flying left). |
157 | // A positive pitch angle moves head downwards (flying forward). |
138 | 158 | // A positive roll angle tilts left side downwards (flying left). |
|
139 | // If compass heading is 0 the head of the copter is in north direction. |
159 | // If compass heading is 0 the head of the copter is in north direction. |
140 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
- | |
141 | // In case of a positive north deviation/velocity the |
160 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
142 | // copter should fly to south (negative pitch). |
161 | // In case of a positive north deviation/velocity the |
143 | // In case of a positive east position deviation and a positive east velocity the |
162 | // copter should fly to south (negative pitch). |
Line 144... | Line -... | ||
144 | // copter should fly to west (positive roll). |
- | |
145 | - | ||
146 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
- | |
147 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
- | |
148 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
- | |
149 | - | ||
150 | // rotation transformation to compass heading to match any copter orientation |
- | |
151 | if (CompassHeading < 0) // no valid compass data |
163 | // In case of a positive east position deviation and a positive east velocity the |
152 | { // disable GPS |
164 | // copter should fly to west (positive roll). |
153 | GPS_Neutral(); |
165 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
154 | } |
166 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
155 | else |
167 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
156 | { |
168 | |
157 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
169 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
158 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
170 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
159 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
171 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
160 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
172 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
161 | } |
173 | |
162 | // limit GPS controls |
174 | // limit GPS controls |
163 | if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
175 | if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
164 | else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
- | |
165 | if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
176 | else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
166 | else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
177 | if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
167 | } |
178 | else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
Line 168... | Line 179... | ||
168 | else // invalid input data |
179 | } |
169 | { |
180 | else // invalid GPS data |
170 | BeepTime = 50; |
181 | { |
- | 182 | GPS_Neutral(); // do nothing |
|
171 | GPS_Neutral(); |
183 | } |
Line 172... | Line 184... | ||
172 | } |
184 | } |
173 | } |
185 | |
174 | 186 | ||
Line 220... | Line 232... | ||
220 | if(HoldPosition.Status != INVALID) |
232 | if(HoldPosition.Status != INVALID) |
221 | { |
233 | { |
222 | // if sticks are centered (no manual control) |
234 | // if sticks are centered (no manual control) |
223 | if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
235 | if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
224 | { |
236 | { |
- | 237 | if(GPS_P_Delay<7) |
|
- | 238 | { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
|
- | 239 | GPS_P_Delay++; |
|
- | 240 | GPS_SetHoldPosition(); // update hold point to current gps position |
|
- | 241 | GPS_PDController(NULL); // activates only the D-Part |
|
- | 242 | } |
|
225 | GPS_PDController(&HoldPosition); |
243 | else GPS_PDController(&HoldPosition);// activates the P&D-Part |
226 | } |
244 | } |
227 | else // MK controlled by user |
245 | else // MK controlled by user |
228 | { |
246 | { |
229 | // update hold point to current gps position |
247 | // update hold point to current gps position |
230 | GPS_SetHoldPosition(); |
248 | GPS_SetHoldPosition(); |
231 | // disable gps control |
249 | // disable gps control |
232 | GPS_Neutral(); |
250 | GPS_Neutral(); |
- | 251 | GPS_P_Delay = 0; |
|
233 | } |
252 | } |
234 | } |
253 | } |
235 | else // invalid Hold Position |
254 | else // invalid Hold Position |
236 | { // try to catch a valid hold position from gps data input |
255 | { // try to catch a valid hold position from gps data input |
237 | GPS_SetHoldPosition(); |
256 | GPS_SetHoldPosition(); |