Rev 746 | Rev 752 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 746 | Rev 747 | ||
---|---|---|---|
1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
2 | 2 | ||
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 | 8 | ||
9 | #define TSK_IDLE 0 |
9 | #define TSK_IDLE 0 |
10 | #define TSK_HOLD 1 |
10 | #define TSK_HOLD 1 |
11 | #define TSK_HOME 2 |
11 | #define TSK_HOME 2 |
12 | 12 | ||
13 | 13 | ||
14 | int16_t GPS_Pitch = 0; |
14 | int16_t GPS_Pitch = 0; |
15 | int16_t GPS_Roll = 0; |
15 | int16_t GPS_Roll = 0; |
16 | 16 | ||
17 | int32_t GPS_P_Factor = 0, GPS_D_Factor = 0; |
17 | int32_t GPS_P_Factor = 0, GPS_D_Factor = 0; |
18 | int32_t GPSPosDev_North = 0, GPSPosDev_East = 0; |
18 | int32_t GPSPosDev_North = 0, GPSPosDev_East = 0; |
19 | 19 | ||
20 | 20 | ||
21 | typedef struct |
21 | typedef struct |
22 | { |
22 | { |
23 | int32_t Northing; |
23 | int32_t Northing; |
24 | int32_t Easting; |
24 | int32_t Easting; |
25 | uint8_t Status; |
25 | uint8_t Status; |
26 | 26 | ||
27 | } GPS_Pos_t; |
27 | } GPS_Pos_t; |
28 | 28 | ||
29 | // GPS coordinates for hold position |
29 | // GPS coordinates for hold position |
30 | GPS_Pos_t HoldPosition = {0,0, INVALID}; |
30 | GPS_Pos_t HoldPosition = {0,0, INVALID}; |
31 | // GPS coordinates for home position |
31 | // GPS coordinates for home position |
32 | GPS_Pos_t HomePosition = {0,0, INVALID}; |
32 | GPS_Pos_t HomePosition = {0,0, INVALID}; |
33 | 33 | ||
34 | 34 | ||
35 | // --------------------------------------------------------------------------------- |
35 | // --------------------------------------------------------------------------------- |
36 | 36 | ||
37 | // set current hold position a home positon |
37 | // set home position to current hold positon |
38 | void GPS_SetHomePosition(void) |
38 | void GPS_SetHomePosition(void) |
39 | { |
39 | { |
40 | HomePosition.Northing = HoldPosition.Northing; |
40 | HomePosition.Northing = HoldPosition.Northing; |
41 | HomePosition.Easting = HoldPosition.Easting; |
41 | HomePosition.Easting = HoldPosition.Easting; |
42 | HomePosition.Status = HoldPosition.Status; |
42 | HomePosition.Status = HoldPosition.Status; |
43 | if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set |
43 | if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set |
44 | } |
44 | } |
45 | 45 | ||
46 | 46 | ||
47 | // disable GPS contrl sticks |
47 | // disable GPS control sticks |
48 | void GPS_Neutral(void) |
48 | void GPS_Neutral(void) |
49 | { |
49 | { |
50 | GPS_Pitch = 0; |
50 | GPS_Pitch = 0; |
51 | GPS_Roll = 0; |
51 | GPS_Roll = 0; |
52 | } |
52 | } |
53 | 53 | ||
54 | // calculates the GPS control sticks salues from the position deviation |
54 | // calculates the GPS control sticks values from the position deviation |
55 | void GPS_PDController(void) |
55 | void GPS_PDController(void) |
56 | { |
56 | { |
57 | int32_t coscompass, sincompass; |
57 | int32_t coscompass, sincompass; |
58 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
58 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
59 | int32_t PD_North = 0, PD_East = 0; |
59 | int32_t PD_North = 0, PD_East = 0; |
60 | //Calculate PD-components of the controller (negative sign for compensation) |
60 | //Calculate PD-components of the controller (negative sign for compensation) |
61 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
61 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
62 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
62 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
63 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
63 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
64 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
64 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
65 | 65 | ||
66 | // PD-controller |
66 | // PD-controller |
67 | PD_North = P_North + D_North; |
67 | PD_North = P_North + D_North; |
68 | PD_East = P_East + D_East; |
68 | PD_East = P_East + D_East; |
69 | 69 | ||
70 | // GPS to pitch and roll settings |
70 | // GPS to pitch and roll settings |
71 | 71 | ||
72 | //A positive pitch angle moves head downwards (flying forward). |
72 | //A positive pitch angle moves head downwards (flying forward). |
73 | //A positive roll angle tilts left side downwards (flying left). |
73 | //A positive roll angle tilts left side downwards (flying left). |
74 | 74 | ||
75 | // If compass heading is 0 the head of the copter is in north direction. |
75 | // If compass heading is 0 the head of the copter is in north direction. |
76 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
76 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
77 | // In case of a positive north deviation/velocity the |
77 | // In case of a positive north deviation/velocity the |
78 | // copter should fly to south (negative pitch). |
78 | // copter should fly to south (negative pitch). |
79 | // In case of a positive east position deviation and a positive east velocity the |
79 | // In case of a positive east position deviation and a positive east velocity the |
80 | // copter should fly to west (positive roll). |
80 | // copter should fly to west (positive roll). |
81 | 81 | ||
82 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
82 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
83 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
83 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
84 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
84 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
85 | 85 | ||
86 | // rotation transformation to compass heading to match any copter orientation |
86 | // rotation transformation to compass heading to match any copter orientation |
87 | if (CompassHeading < 0) // no valid compass data |
87 | if (CompassHeading < 0) // no valid compass data |
88 | { // disable GPS |
88 | { // disable GPS |
89 | GPS_Neutral(); |
89 | GPS_Neutral(); |
90 | } |
90 | } |
91 | else |
91 | else |
92 | { |
92 | { |
93 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
93 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
94 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
94 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
95 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
95 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
96 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
96 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
97 | } |
97 | } |
98 | // limit GPS controls |
98 | // limit GPS controls |
99 | #define GPS_CTRL_LIMIT 35 |
99 | #define GPS_CTRL_LIMIT 35 |
100 | if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
100 | if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
101 | if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
101 | if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
102 | if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
102 | if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
103 | if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
103 | if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
104 | } |
104 | } |
105 | 105 | ||
106 | 106 | ||
107 | void GPS_Main(void) |
107 | void GPS_Main(void) |
108 | { |
108 | { |
109 | static uint8_t GPS_Task = TSK_IDLE; |
109 | static uint8_t GPS_Task = TSK_IDLE; |
110 | 110 | ||
111 | 111 | ||
112 | // poti2 enables the gps feature |
112 | // poti2 enables the gps feature |
113 | if(Poti2 < 70) GPS_Task = TSK_IDLE; |
113 | if(Poti2 < 70) GPS_Task = TSK_IDLE; |
114 | else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
114 | else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
115 | else GPS_Task = TSK_HOME; // Poti2 >= 160 |
115 | else GPS_Task = TSK_HOME; // Poti2 >= 160 |
116 | 116 | ||
117 | 117 | ||
118 | switch(GPSInfo.status) |
118 | switch(GPSInfo.status) |
119 | { |
119 | { |
120 | case INVALID: // invalid gps data |
120 | case INVALID: // invalid gps data |
121 | GPS_Neutral(); |
121 | GPS_Neutral(); |
122 | if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary |
122 | if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary |
123 | break; |
123 | break; |
124 | case PROCESSED: // if gps data are already processed |
124 | case PROCESSED: // if gps data are already processed |
125 | // downcount timeout |
125 | // downcount timeout |
126 | if(GPSTimeout) GPSTimeout--; |
126 | if(GPSTimeout) GPSTimeout--; |
127 | // if no new data arrived within timeout set current data invalid |
127 | // if no new data arrived within timeout set current data invalid |
128 | // and therefore disable GPS |
128 | // and therefore disable GPS |
129 | else |
129 | else |
130 | { |
130 | { |
131 | GPS_Neutral(); |
131 | GPS_Neutral(); |
132 | GPSInfo.status = INVALID; |
132 | GPSInfo.status = INVALID; |
133 | } |
133 | } |
134 | break; |
134 | break; |
135 | case VALID: // new valid data from gps device |
135 | case VALID: // new valid data from gps device |
136 | // if the gps data quality is sufficient |
136 | // if the gps data quality is sufficient |
137 | if (GPSInfo.satfix == SATFIX_3D) |
137 | if (GPSInfo.satfix == SATFIX_3D) |
138 | { |
138 | { |
139 | switch(GPS_Task) // check what's to do |
139 | switch(GPS_Task) // check what's to do |
140 | { |
140 | { |
141 | case TSK_IDLE: |
141 | case TSK_IDLE: |
142 | // update hold point to current gps position |
142 | // update hold point to current gps position |
143 | HoldPosition.Northing = GPSInfo.utmnorth; |
143 | HoldPosition.Northing = GPSInfo.utmnorth; |
144 | HoldPosition.Easting = GPSInfo.utmeast; |
144 | HoldPosition.Easting = GPSInfo.utmeast; |
145 | HoldPosition.Status = VALID; |
145 | HoldPosition.Status = VALID; |
146 | // disable gps control |
146 | // disable gps control |
147 | GPS_Neutral(); |
147 | GPS_Neutral(); |
148 | break; // eof TSK_IDLE |
148 | break; // eof TSK_IDLE |
149 | case TSK_HOLD: |
149 | case TSK_HOLD: |
150 | // if sticks are centered and hold position is valid enable position hold control |
150 | // if sticks are centered and hold position is valid enable position hold control |
151 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
151 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
152 | { |
152 | { |
153 | // Calculate deviation from hold position |
153 | // Calculate deviation from hold position |
154 | GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
154 | GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
155 | GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
155 | GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
156 | DebugOut.Analog[12] = GPSPosDev_North; |
156 | DebugOut.Analog[12] = GPSPosDev_North; |
157 | DebugOut.Analog[13] = GPSPosDev_East; |
157 | DebugOut.Analog[13] = GPSPosDev_East; |
158 | 158 | ||
159 | GPS_PDController(); |
159 | GPS_PDController(); |
160 | } |
160 | } |
161 | else // MK controlled by user |
161 | else // MK controlled by user |
162 | { |
162 | { |
163 | // update hold point to current gps position |
163 | // update hold point to current gps position |
164 | HoldPosition.Northing = GPSInfo.utmnorth; |
164 | HoldPosition.Northing = GPSInfo.utmnorth; |
165 | HoldPosition.Easting = GPSInfo.utmeast; |
165 | HoldPosition.Easting = GPSInfo.utmeast; |
166 | HoldPosition.Status = VALID; |
166 | HoldPosition.Status = VALID; |
167 | // disable gps control |
167 | // disable gps control |
168 | GPS_Neutral(); |
168 | GPS_Neutral(); |
169 | } |
169 | } |
170 | break; // eof TSK_HOLD |
170 | break; // eof TSK_HOLD |
171 | case TSK_HOME: |
171 | case TSK_HOME: |
172 | if(HomePosition.Status == VALID) |
172 | if(HomePosition.Status == VALID) |
173 | { |
173 | { |
174 | // update hold point to current gps position |
174 | // update hold point to current gps position |
175 | HoldPosition.Northing = GPSInfo.utmnorth; |
175 | HoldPosition.Northing = GPSInfo.utmnorth; |
176 | HoldPosition.Easting = GPSInfo.utmeast; |
176 | HoldPosition.Easting = GPSInfo.utmeast; |
177 | HoldPosition.Status = VALID; |
177 | HoldPosition.Status = VALID; |
178 | 178 | ||
179 | // Calculate deviation from home position |
179 | // Calculate deviation from home position |
180 | GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing; |
180 | GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing; |
181 | GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting; |
181 | GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting; |
182 | DebugOut.Analog[12] = GPSPosDev_North; |
182 | DebugOut.Analog[12] = GPSPosDev_North; |
183 | DebugOut.Analog[13] = GPSPosDev_East; |
183 | DebugOut.Analog[13] = GPSPosDev_East; |
184 | 184 | ||
185 | GPS_PDController(); |
185 | GPS_PDController(); |
186 | } |
186 | } |
187 | else // bad home position |
187 | else // bad home position |
188 | { |
188 | { |
189 | GPS_Neutral(); |
189 | GPS_Neutral(); |
190 | BeepTime = 50; // signal invalid home position |
190 | BeepTime = 50; // signal invalid home position |
191 | } |
191 | } |
192 | break; // eof TSK_HOME |
192 | break; // eof TSK_HOME |
193 | default: // unhandled task |
193 | default: // unhandled task |
194 | GPS_Neutral(); |
194 | GPS_Neutral(); |
195 | break; // eof default |
195 | break; // eof default |
196 | } // eof switch GPS_Task |
196 | } // eof switch GPS_Task |
197 | } // eof 3D-FIX |
197 | } // eof 3D-FIX |
198 | 198 | ||
199 | else // no 3D-SATFIX |
199 | else // no 3D-SATFIX |
200 | { // disable gps control |
200 | { // disable gps control |
201 | GPS_Neutral(); |
201 | GPS_Neutral(); |
202 | if(GPS_Task != TSK_IDLE) BeepTime = 50; |
202 | if(GPS_Task != TSK_IDLE) BeepTime = 50; |
203 | } |
203 | } |
204 | 204 | ||
205 | // set current data as processed to avoid further calculations on the same gps data |
205 | // set current data as processed to avoid further calculations on the same gps data |
206 | GPSInfo.status = PROCESSED; |
206 | GPSInfo.status = PROCESSED; |
207 | break; |
207 | break; |
208 | } // eof GPSInfo.status |
208 | } // eof GPSInfo.status |
209 | DebugOut.Analog[14] = GPS_Pitch; |
209 | DebugOut.Analog[14] = GPS_Pitch; |
210 | DebugOut.Analog[15] = GPS_Roll; |
210 | DebugOut.Analog[15] = GPS_Roll; |
211 | } |
211 | } |
212 | 212 | ||
213 | 213 |