Rev 752 | Rev 764 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 752 | Rev 762 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
| 2 | - | ||
| 3 | #include "fc.h" |
2 | #include "fc.h" |
| 4 | #include "ubx.h" |
3 | #include "ubx.h" |
| 5 | #include "mymath.h" |
4 | #include "mymath.h" |
| 6 | #include "timer0.h" |
5 | #include "timer0.h" |
| 7 | #include "uart.h" |
6 | #include "uart.h" |
| Line 8... | Line 7... | ||
| 8 | 7 | ||
| 9 | #define TSK_IDLE 0 |
8 | #define TSK_IDLE 0 |
| 10 | #define TSK_HOLD 1 |
9 | #define TSK_HOLD 1 |
| Line -... | Line 10... | ||
| - | 10 | #define TSK_HOME 2 |
|
| - | 11 | ||
| Line 11... | Line 12... | ||
| 11 | #define TSK_HOME 2 |
12 | #define GPS_STICK_SENSE 12 |
| 12 | 13 | #define GPS_STICK_LIMIT 45 |
|
| Line 13... | Line 14... | ||
| 13 | 14 | ||
| Line 41... | Line 42... | ||
| 41 | HomePosition.Easting = HoldPosition.Easting; |
42 | HomePosition.Easting = HoldPosition.Easting; |
| 42 | HomePosition.Status = HoldPosition.Status; |
43 | HomePosition.Status = HoldPosition.Status; |
| 43 | if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set |
44 | if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set |
| 44 | } |
45 | } |
| Line -... | Line 46... | ||
| - | 46 | ||
| - | 47 | // clear home position |
|
| - | 48 | void GPS_ClearHomePosition(void) |
|
| - | 49 | { |
|
| - | 50 | HomePosition.Status = INVALID; |
|
| Line 45... | Line 51... | ||
| 45 | 51 | } |
|
| 46 | 52 | ||
| 47 | // disable GPS control sticks |
53 | // disable GPS control sticks |
| 48 | void GPS_Neutral(void) |
54 | void GPS_Neutral(void) |
| 49 | { |
55 | { |
| 50 | GPS_Pitch = 0; |
56 | GPS_Pitch = 0; |
| Line 51... | Line 57... | ||
| 51 | GPS_Roll = 0; |
57 | GPS_Roll = 0; |
| 52 | } |
58 | } |
| 53 | 59 | ||
| 54 | // calculates the GPS control sticks values from the position deviation |
60 | // calculates the GPS control sticks values from the deviation to target position |
| 55 | void GPS_PDController(void) |
61 | void GPS_PDController(GPS_Pos_t *TargetPos) |
| 56 | { |
62 | { |
| - | 63 | int32_t coscompass, sincompass; |
|
| - | 64 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
|
| - | 65 | int32_t PD_North = 0, PD_East = 0; |
|
| - | 66 | ||
| - | 67 | if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
|
| - | 68 | { |
|
| - | 69 | GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing; |
|
| - | 70 | GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting; |
|
| - | 71 | ||
| 57 | int32_t coscompass, sincompass; |
72 | DebugOut.Analog[12] = GPSPosDev_North; |
| 58 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
73 | DebugOut.Analog[13] = GPSPosDev_East; |
| 59 | int32_t PD_North = 0, PD_East = 0; |
74 | |
| 60 | //Calculate PD-components of the controller (negative sign for compensation) |
75 | //Calculate PD-components of the controller (negative sign for compensation) |
| 61 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
76 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| Line 94... | Line 109... | ||
| 94 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
109 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
| 95 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
110 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
| 96 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
111 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
| 97 | } |
112 | } |
| 98 | // limit GPS controls |
113 | // limit GPS controls |
| 99 | #define GPS_CTRL_LIMIT 35 |
- | |
| 100 | if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
114 | if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
| 101 | if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
115 | if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
| 102 | if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
116 | if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
| 103 | if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
117 | if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
| - | 118 | } |
|
| - | 119 | else // invalid input data |
|
| - | 120 | { |
|
| - | 121 | BeepTime = 50; |
|
| - | 122 | GPS_Neutral(); |
|
| - | 123 | } |
|
| 104 | } |
124 | } |
| Line 105... | Line 125... | ||
| 105 | 125 | ||
| 106 | 126 | ||
| 107 | void GPS_Main(void) |
127 | void GPS_Main(uint8_t ctrl) |
| 108 | { |
128 | { |
| Line 109... | Line 129... | ||
| 109 | static uint8_t GPS_Task = TSK_IDLE; |
129 | static uint8_t GPS_Task = TSK_IDLE; |
| 110 | int16_t satbeep; |
130 | int16_t satbeep; |
| 111 | 131 | ||
| 112 | // poti2 enables the gps feature |
132 | // ctrl enables the gps feature |
| Line 113... | Line 133... | ||
| 113 | if(Poti2 < 70) GPS_Task = TSK_IDLE; |
133 | if(ctrl < 70) GPS_Task = TSK_IDLE; |
| 114 | else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
134 | else if (ctrl < 160) GPS_Task = TSK_HOLD; |
| 115 | else GPS_Task = TSK_HOME; // Poti2 >= 160 |
135 | else GPS_Task = TSK_HOME; // ctrl >= 160 |
| Line 148... | Line 168... | ||
| 148 | HoldPosition.Status = VALID; |
168 | HoldPosition.Status = VALID; |
| 149 | // disable gps control |
169 | // disable gps control |
| 150 | GPS_Neutral(); |
170 | GPS_Neutral(); |
| 151 | break; // eof TSK_IDLE |
171 | break; // eof TSK_IDLE |
| 152 | case TSK_HOLD: |
172 | case TSK_HOLD: |
| 153 | // if sticks are centered and hold position is valid enable position hold control |
- | |
| 154 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
173 | if(HoldPosition.Status == VALID) |
| 155 | { |
174 | { |
| 156 | // Calculate deviation from hold position |
175 | // if sticks are centered (no manual control) |
| 157 | GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
176 | if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
| 158 | GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
- | |
| 159 | DebugOut.Analog[12] = GPSPosDev_North; |
- | |
| 160 | DebugOut.Analog[13] = GPSPosDev_East; |
- | |
| 161 | 177 | { |
|
| 162 | GPS_PDController(); |
178 | GPS_PDController(&HoldPosition); |
| 163 | } |
179 | } |
| 164 | else // MK controlled by user |
180 | else // MK controlled by user |
| 165 | { |
181 | { |
| 166 | // update hold point to current gps position |
182 | // update hold point to current gps position |
| 167 | HoldPosition.Northing = GPSInfo.utmnorth; |
183 | HoldPosition.Northing = GPSInfo.utmnorth; |
| 168 | HoldPosition.Easting = GPSInfo.utmeast; |
184 | HoldPosition.Easting = GPSInfo.utmeast; |
| 169 | HoldPosition.Status = VALID; |
185 | HoldPosition.Status = GPSInfo.status; |
| 170 | // disable gps control |
186 | // disable gps control |
| 171 | GPS_Neutral(); |
187 | GPS_Neutral(); |
| 172 | } |
188 | } |
| - | 189 | } |
|
| 173 | break; // eof TSK_HOLD |
190 | break; // eof TSK_HOLD |
| 174 | case TSK_HOME: |
191 | case TSK_HOME: |
| 175 | if(HomePosition.Status == VALID) |
192 | if(HomePosition.Status == VALID) |
| 176 | { |
193 | { |
| 177 | // update hold point to current gps position |
194 | // update hold point to current gps position |
| - | 195 | // to avoid a flight back if home comming is deactivated |
|
| 178 | HoldPosition.Northing = GPSInfo.utmnorth; |
196 | HoldPosition.Northing = GPSInfo.utmnorth; |
| 179 | HoldPosition.Easting = GPSInfo.utmeast; |
197 | HoldPosition.Easting = GPSInfo.utmeast; |
| 180 | HoldPosition.Status = VALID; |
198 | HoldPosition.Status = GPSInfo.status; |
| 181 | - | ||
| 182 | // Calculate deviation from home position |
199 | // if sticks are centered (no manual control) |
| 183 | GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing; |
200 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
| 184 | GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting; |
- | |
| 185 | DebugOut.Analog[12] = GPSPosDev_North; |
201 | { |
| 186 | DebugOut.Analog[13] = GPSPosDev_East; |
202 | GPS_PDController(&HomePosition); |
| 187 | - | ||
| 188 | GPS_PDController(); |
203 | } |
| 189 | } |
204 | } |
| 190 | else // bad home position |
205 | else // bad home position |
| 191 | { |
206 | { |
| 192 | GPS_Neutral(); |
- | |
| 193 | BeepTime = 50; // signal invalid home position |
207 | BeepTime = 50; // signal invalid home position |
| - | 208 | // try to hold at least the position as a fallback option |
|
| - | 209 | // if sticks are centered (no manual control) |
|
| - | 210 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID)) |
|
| - | 211 | { |
|
| - | 212 | GPS_PDController(&HoldPosition); |
|
| - | 213 | } |
|
| 194 | } |
214 | } |
| 195 | break; // eof TSK_HOME |
215 | break; // eof TSK_HOME |
| 196 | default: // unhandled task |
216 | default: // unhandled task |
| 197 | GPS_Neutral(); |
217 | GPS_Neutral(); |
| 198 | break; // eof default |
218 | break; // eof default |