Rev 792 | Rev 806 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
726 | killagreg | 1 | #include <inttypes.h> |
764 | killagreg | 2 | #include <stdlib.h> |
726 | killagreg | 3 | #include "fc.h" |
4 | #include "ubx.h" |
||
5 | #include "mymath.h" |
||
727 | killagreg | 6 | #include "timer0.h" |
741 | killagreg | 7 | #include "uart.h" |
794 | killagreg | 8 | #include "rc.h" |
9 | #include "eeprom.h" |
||
701 | killagreg | 10 | |
741 | killagreg | 11 | #define TSK_IDLE 0 |
12 | #define TSK_HOLD 1 |
||
13 | #define TSK_HOME 2 |
||
14 | |||
767 | killagreg | 15 | #define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function |
16 | #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
||
782 | killagreg | 17 | #define GPS_D_LIMIT_DIST 500 // for position deviations greater than 500cm the D-Part of the PD-Controller is limited |
767 | killagreg | 18 | #define GPS_D_LIMIT 50 // PD-Controntroller D-Limit. |
741 | killagreg | 19 | |
726 | killagreg | 20 | int16_t GPS_Pitch = 0; |
21 | int16_t GPS_Roll = 0; |
||
22 | |||
746 | killagreg | 23 | int32_t GPS_P_Factor = 0, GPS_D_Factor = 0; |
726 | killagreg | 24 | |
746 | killagreg | 25 | |
781 | killagreg | 26 | |
726 | killagreg | 27 | typedef struct |
1 | ingob | 28 | { |
781 | killagreg | 29 | int32_t Longitude; |
30 | int32_t Latitude; |
||
794 | killagreg | 31 | int32_t Altitude; |
726 | killagreg | 32 | uint8_t Status; |
33 | } GPS_Pos_t; |
||
34 | |||
35 | // GPS coordinates for hold position |
||
794 | killagreg | 36 | GPS_Pos_t HoldPosition = {0,0,0,INVALID}; |
741 | killagreg | 37 | // GPS coordinates for home position |
794 | killagreg | 38 | GPS_Pos_t HomePosition = {0,0,0,INVALID}; |
726 | killagreg | 39 | |
40 | |||
41 | // --------------------------------------------------------------------------------- |
||
42 | |||
794 | killagreg | 43 | // checks pitch and roll sticks for manual control |
44 | uint8_t IsManualControlled(void) |
||
45 | { |
||
46 | if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0; |
||
47 | else return 1; |
||
48 | } |
||
49 | |||
781 | killagreg | 50 | // set home position to current positon |
746 | killagreg | 51 | void GPS_SetHomePosition(void) |
52 | { |
||
781 | killagreg | 53 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
54 | { |
||
55 | HomePosition.Longitude = GPSInfo.longitude; |
||
56 | HomePosition.Latitude = GPSInfo.latitude; |
||
794 | killagreg | 57 | HomePosition.Altitude = GPSInfo.altitude; |
781 | killagreg | 58 | HomePosition.Status = VALID; |
59 | BeepTime = 1000; // signal if new home position was set |
||
60 | } |
||
61 | else |
||
62 | { |
||
63 | HomePosition.Status = INVALID; |
||
64 | } |
||
746 | killagreg | 65 | } |
726 | killagreg | 66 | |
781 | killagreg | 67 | // set hold position to current positon |
68 | void GPS_SetHoldPosition(void) |
||
69 | { |
||
70 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
||
71 | { |
||
72 | HoldPosition.Longitude = GPSInfo.longitude; |
||
73 | HoldPosition.Latitude = GPSInfo.latitude; |
||
794 | killagreg | 74 | HoldPosition.Altitude = GPSInfo.altitude; |
781 | killagreg | 75 | HoldPosition.Status = VALID; |
76 | } |
||
77 | else |
||
78 | { |
||
79 | HoldPosition.Status = INVALID; |
||
80 | } |
||
81 | } |
||
82 | |||
762 | killagreg | 83 | // clear home position |
84 | void GPS_ClearHomePosition(void) |
||
85 | { |
||
86 | HomePosition.Status = INVALID; |
||
87 | } |
||
746 | killagreg | 88 | |
747 | killagreg | 89 | // disable GPS control sticks |
746 | killagreg | 90 | void GPS_Neutral(void) |
1 | ingob | 91 | { |
746 | killagreg | 92 | GPS_Pitch = 0; |
93 | GPS_Roll = 0; |
||
94 | } |
||
95 | |||
792 | killagreg | 96 | // calculates the GPS control stick values from the deviation to target position |
97 | // if the pointer to the target positin is NULL or is the target position invalid |
||
98 | // then the P part of the controller is deactivated. |
||
99 | void GPS_PDController(GPS_Pos_t *pTargetPos) |
||
746 | killagreg | 100 | { |
726 | killagreg | 101 | int32_t coscompass, sincompass; |
781 | killagreg | 102 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
726 | killagreg | 103 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
104 | int32_t PD_North = 0, PD_East = 0; |
||
781 | killagreg | 105 | static int32_t cos_target_latitude = 1; |
746 | killagreg | 106 | |
792 | killagreg | 107 | // if GPS data and Compass are ok |
108 | if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) ) |
||
109 | { |
||
781 | killagreg | 110 | |
792 | killagreg | 111 | if(pTargetPos != NULL) // if there is a target position |
781 | killagreg | 112 | { |
792 | killagreg | 113 | if(pTargetPos->Status != INVALID) // and the position data are valid |
114 | { // calculate position deviation from latitude and longitude differences |
||
115 | GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
||
116 | GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
||
117 | // recalculate the target latitude projection only if the target data are updated |
||
118 | // to save time |
||
119 | if (pTargetPos->Status != PROCESSED) |
||
120 | { |
||
121 | cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
||
122 | pTargetPos->Status = PROCESSED; |
||
123 | } |
||
124 | // calculate latitude projection |
||
125 | GPSPosDev_East *= cos_target_latitude; |
||
126 | GPSPosDev_East /= 8192; |
||
746 | killagreg | 127 | |
792 | killagreg | 128 | DebugOut.Analog[12] = GPSPosDev_North; |
129 | DebugOut.Analog[13] = GPSPosDev_East; |
||
130 | //DebugOut.Analog[12] = GPSInfo.velnorth; |
||
131 | //DebugOut.Analog[13] = GPSInfo.veleast; |
||
746 | killagreg | 132 | |
792 | killagreg | 133 | //Calculate P-components of the controller (negative sign for compensation) |
134 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
||
135 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
||
136 | } |
||
137 | else // not valid target position available |
||
138 | { |
||
139 | GPSPosDev_North = 0; |
||
140 | GPSPosDev_East = 0; |
||
141 | } |
||
142 | } |
||
143 | else // not target position available |
||
144 | { |
||
145 | GPSPosDev_North = 0; |
||
146 | GPSPosDev_East = 0; |
||
147 | } |
||
767 | killagreg | 148 | |
762 | killagreg | 149 | //Calculate PD-components of the controller (negative sign for compensation) |
150 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
||
792 | killagreg | 151 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
762 | killagreg | 152 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
153 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
||
792 | killagreg | 154 | // limit D-Part if position deviation is significant of the target position |
155 | // this accelerates flying to the target position |
||
764 | killagreg | 156 | if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) ) |
157 | { |
||
158 | if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT; |
||
159 | else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT; |
||
160 | if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT; |
||
161 | else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT; |
||
162 | } |
||
762 | killagreg | 163 | // PD-controller |
164 | PD_North = P_North + D_North; |
||
165 | PD_East = P_East + D_East; |
||
746 | killagreg | 166 | |
762 | killagreg | 167 | // GPS to pitch and roll settings |
746 | killagreg | 168 | |
792 | killagreg | 169 | // A positive pitch angle moves head downwards (flying forward). |
170 | // A positive roll angle tilts left side downwards (flying left). |
||
762 | killagreg | 171 | // If compass heading is 0 the head of the copter is in north direction. |
172 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
||
173 | // In case of a positive north deviation/velocity the |
||
174 | // copter should fly to south (negative pitch). |
||
175 | // In case of a positive east position deviation and a positive east velocity the |
||
176 | // copter should fly to west (positive roll). |
||
177 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
||
178 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
||
179 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
||
180 | |||
792 | killagreg | 181 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
182 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
||
183 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
||
184 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
||
185 | |||
762 | killagreg | 186 | // limit GPS controls |
187 | if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
||
764 | killagreg | 188 | else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
762 | killagreg | 189 | if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
764 | killagreg | 190 | else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
746 | killagreg | 191 | } |
792 | killagreg | 192 | else // invalid GPS data |
746 | killagreg | 193 | { |
792 | killagreg | 194 | GPS_Neutral(); // do nothing |
746 | killagreg | 195 | } |
196 | } |
||
197 | |||
198 | |||
762 | killagreg | 199 | void GPS_Main(uint8_t ctrl) |
746 | killagreg | 200 | { |
741 | killagreg | 201 | static uint8_t GPS_Task = TSK_IDLE; |
792 | killagreg | 202 | static uint8_t GPS_P_Delay = 0; |
752 | killagreg | 203 | int16_t satbeep; |
1 | ingob | 204 | |
762 | killagreg | 205 | // ctrl enables the gps feature |
206 | if(ctrl < 70) GPS_Task = TSK_IDLE; |
||
207 | else if (ctrl < 160) GPS_Task = TSK_HOLD; |
||
208 | else GPS_Task = TSK_HOME; // ctrl >= 160 |
||
741 | killagreg | 209 | |
210 | |||
211 | switch(GPSInfo.status) |
||
726 | killagreg | 212 | { |
741 | killagreg | 213 | case INVALID: // invalid gps data |
746 | killagreg | 214 | GPS_Neutral(); |
752 | killagreg | 215 | if(GPS_Task != TSK_IDLE) |
216 | { |
||
217 | BeepTime = 100; // beep if signal is neccesary |
||
218 | } |
||
741 | killagreg | 219 | break; |
781 | killagreg | 220 | case PROCESSED: // if gps data are already processed do nothing |
741 | killagreg | 221 | // downcount timeout |
222 | if(GPSTimeout) GPSTimeout--; |
||
223 | // if no new data arrived within timeout set current data invalid |
||
224 | // and therefore disable GPS |
||
225 | else |
||
726 | killagreg | 226 | { |
746 | killagreg | 227 | GPS_Neutral(); |
741 | killagreg | 228 | GPSInfo.status = INVALID; |
229 | } |
||
230 | break; |
||
231 | case VALID: // new valid data from gps device |
||
781 | killagreg | 232 | // if the gps data quality is good |
741 | killagreg | 233 | if (GPSInfo.satfix == SATFIX_3D) |
234 | { |
||
235 | switch(GPS_Task) // check what's to do |
||
726 | killagreg | 236 | { |
741 | killagreg | 237 | case TSK_IDLE: |
781 | killagreg | 238 | // update hold position to current gps position |
239 | GPS_SetHoldPosition(); // can get invalid if gps signal is bad |
||
741 | killagreg | 240 | // disable gps control |
746 | killagreg | 241 | GPS_Neutral(); |
741 | killagreg | 242 | break; // eof TSK_IDLE |
243 | case TSK_HOLD: |
||
781 | killagreg | 244 | if(HoldPosition.Status != INVALID) |
741 | killagreg | 245 | { |
794 | killagreg | 246 | if( IsManualControlled() ) // MK controlled by user |
762 | killagreg | 247 | { |
794 | killagreg | 248 | // update hold point to current gps position |
249 | GPS_SetHoldPosition(); |
||
250 | // disable gps control |
||
251 | GPS_Neutral(); |
||
252 | GPS_P_Delay = 0; |
||
253 | } |
||
254 | else // GPS control active |
||
255 | { |
||
792 | killagreg | 256 | if(GPS_P_Delay<7) |
257 | { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
||
258 | GPS_P_Delay++; |
||
259 | GPS_SetHoldPosition(); // update hold point to current gps position |
||
260 | GPS_PDController(NULL); // activates only the D-Part |
||
261 | } |
||
262 | else GPS_PDController(&HoldPosition);// activates the P&D-Part |
||
762 | killagreg | 263 | } |
741 | killagreg | 264 | } |
781 | killagreg | 265 | else // invalid Hold Position |
266 | { // try to catch a valid hold position from gps data input |
||
267 | GPS_SetHoldPosition(); |
||
268 | GPS_Neutral(); |
||
269 | } |
||
741 | killagreg | 270 | break; // eof TSK_HOLD |
746 | killagreg | 271 | case TSK_HOME: |
781 | killagreg | 272 | if(HomePosition.Status != INVALID) |
746 | killagreg | 273 | { |
274 | // update hold point to current gps position |
||
762 | killagreg | 275 | // to avoid a flight back if home comming is deactivated |
781 | killagreg | 276 | GPS_SetHoldPosition(); |
794 | killagreg | 277 | if( IsManualControlled() ) // MK controlled by user |
762 | killagreg | 278 | { |
794 | killagreg | 279 | GPS_Neutral(); |
762 | killagreg | 280 | } |
794 | killagreg | 281 | else // GPS control active |
767 | killagreg | 282 | { |
794 | killagreg | 283 | GPS_PDController(&HomePosition); |
767 | killagreg | 284 | } |
746 | killagreg | 285 | } |
286 | else // bad home position |
||
287 | { |
||
288 | BeepTime = 50; // signal invalid home position |
||
762 | killagreg | 289 | // try to hold at least the position as a fallback option |
781 | killagreg | 290 | |
291 | if (HoldPosition.Status != INVALID) |
||
762 | killagreg | 292 | { |
794 | killagreg | 293 | if( IsManualControlled() ) // MK controlled by user |
781 | killagreg | 294 | { |
794 | killagreg | 295 | GPS_Neutral(); |
781 | killagreg | 296 | } |
794 | killagreg | 297 | else // GPS control active |
781 | killagreg | 298 | { |
794 | killagreg | 299 | GPS_PDController(&HoldPosition); |
781 | killagreg | 300 | } |
762 | killagreg | 301 | } |
781 | killagreg | 302 | else |
303 | { // try to catch a valid hold position |
||
304 | GPS_SetHoldPosition(); |
||
767 | killagreg | 305 | GPS_Neutral(); |
306 | } |
||
746 | killagreg | 307 | } |
308 | break; // eof TSK_HOME |
||
741 | killagreg | 309 | default: // unhandled task |
746 | killagreg | 310 | GPS_Neutral(); |
741 | killagreg | 311 | break; // eof default |
312 | } // eof switch GPS_Task |
||
313 | } // eof 3D-FIX |
||
314 | else // no 3D-SATFIX |
||
315 | { // disable gps control |
||
746 | killagreg | 316 | GPS_Neutral(); |
752 | killagreg | 317 | if(GPS_Task != TSK_IDLE) |
318 | { |
||
767 | killagreg | 319 | satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats |
752 | killagreg | 320 | if (satbeep < 0) satbeep = 0; |
767 | killagreg | 321 | BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms = |
752 | killagreg | 322 | } |
741 | killagreg | 323 | } |
324 | // set current data as processed to avoid further calculations on the same gps data |
||
325 | GPSInfo.status = PROCESSED; |
||
326 | break; |
||
327 | } // eof GPSInfo.status |
||
328 | DebugOut.Analog[14] = GPS_Pitch; |
||
329 | DebugOut.Analog[15] = GPS_Roll; |
||
726 | killagreg | 330 | } |
331 |