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