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