Rev 733 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 733 | Rev 741 | ||
|---|---|---|---|
| Line 2... | Line 2... | ||
| 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" |
|
| - | 7 | #include "uart.h" |
|
| - | 8 | ||
| - | 9 | #define TSK_IDLE 0 |
|
| - | 10 | #define TSK_HOLD 1 |
|
| - | 11 | #define TSK_HOME 2 |
|
| Line 6... | Line 12... | ||
| 6 | #include "timer0.h" |
12 | |
| 7 | 13 | ||
| Line 8... | Line 14... | ||
| 8 | int16_t GPS_Pitch = 0; |
14 | int16_t GPS_Pitch = 0; |
| Line 18... | Line 24... | ||
| 18 | uint8_t Status; |
24 | uint8_t Status; |
| Line 19... | Line 25... | ||
| 19 | 25 | ||
| Line 20... | Line 26... | ||
| 20 | } GPS_Pos_t; |
26 | } GPS_Pos_t; |
| 21 | 27 | ||
| 22 | // GPS coordinates for hold position |
- | |
| - | 28 | // GPS coordinates for hold position |
|
| - | 29 | GPS_Pos_t HoldPosition = {0,0, INVALID}; |
|
| Line 23... | Line 30... | ||
| 23 | GPS_Pos_t GPSHoldPoint = {0,0, INVALID}; |
30 | // GPS coordinates for home position |
| Line 31... | Line 38... | ||
| 31 | { |
38 | { |
| 32 | int32_t coscompass, sincompass; |
39 | int32_t coscompass, sincompass; |
| 33 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
40 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
| 34 | int32_t PD_North = 0, PD_East = 0; |
41 | int32_t PD_North = 0, PD_East = 0; |
| 35 | int32_t GPSPosDev_North = 0, GPSPosDev_East = 0; |
42 | int32_t GPSPosDev_North = 0, GPSPosDev_East = 0; |
| - | 43 | static uint8_t GPS_Task = TSK_IDLE; |
|
| - | 44 | ||
| Line 36... | Line 45... | ||
| 36 | 45 | ||
| 37 | // poti2 enables the gps feature |
46 | // poti2 enables the gps feature |
| - | 47 | if(Poti2 < 70) GPS_Task = TSK_IDLE; |
|
| - | 48 | else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
|
| - | 49 | //else GPS_Task = TSK_HOME; |
|
| - | 50 | ||
| - | 51 | ||
| 38 | if((Poti2 > 70) && (!EmergencyLanding)) // run GPS function only if Poti 2 is larger than 70 (switch on) |
52 | switch(GPSInfo.status) |
| - | 53 | { |
|
| - | 54 | case INVALID: // invalid gps data |
|
| - | 55 | GPS_Pitch = 0; |
|
| - | 56 | GPS_Roll = 0; |
|
| - | 57 | if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary |
|
| - | 58 | break; |
|
| 39 | { |
59 | case PROCESSED: // if gps data are already processed |
| - | 60 | // downcount timeout |
|
| - | 61 | if(GPSTimeout) GPSTimeout--; |
|
| - | 62 | // if no new data arrived within timeout set current data invalid |
|
| - | 63 | // and therefore disable GPS |
|
| 40 | switch(GPSInfo.status) |
64 | else |
| 41 | { |
- | |
| 42 | case INVALID: // invalid gps data |
65 | { |
| 43 | GPS_Pitch = 0; |
66 | GPS_Pitch = 0; |
| 44 | GPS_Roll = 0; |
- | |
| 45 | BeepTime = 50; |
- | |
| 46 | break; |
- | |
| 47 | case PROCESSED: // if gps data are already processed |
- | |
| 48 | // downcount timeout |
- | |
| 49 | if(GPSTimeout) GPSTimeout--; |
- | |
| 50 | // if no new data arrived within timeout set current data invalid |
- | |
| 51 | // and therefore disable GPS |
- | |
| 52 | else |
- | |
| 53 | { |
- | |
| 54 | GPS_Pitch = 0; |
- | |
| 55 | GPS_Roll = 0; |
67 | GPS_Roll = 0; |
| 56 | GPSInfo.status = INVALID; |
68 | GPSInfo.status = INVALID; |
| 57 | } |
69 | } |
| 58 | break; |
70 | break; |
| 59 | case VALID: // new valid data from gps device |
71 | case VALID: // new valid data from gps device |
| 60 | // if the gps data quality is sufficient |
72 | // if the gps data quality is sufficient |
| - | 73 | if (GPSInfo.satfix == SATFIX_3D) |
|
| - | 74 | { |
|
| 61 | if (GPSInfo.satfix == SATFIX_3D) |
75 | switch(GPS_Task) // check what's to do |
| 62 | { |
- | |
| 63 | // if sticks are centered and hold position is valid enable position hold control |
- | |
| 64 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID)) |
- | |
| 65 | { |
- | |
| 66 | // Calculate deviation from hold position |
- | |
| 67 | GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing; |
- | |
| 68 | GPSPosDev_East = GPSInfo.utmeast - GPSHoldPoint.Easting; |
- | |
| 69 | - | ||
| 70 | //Calculate PD-components of the controller (negative sign for compensation) |
- | |
| 71 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
- | |
| 72 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
- | |
| 73 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
- | |
| 74 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
- | |
| 75 | 76 | { |
|
| 76 | // PD-controller |
- | |
| 77 | PD_North = P_North + D_North; |
- | |
| 78 | PD_East = P_East + D_East; |
- | |
| 79 | - | ||
| 80 | // GPS to pitch and roll settings |
- | |
| 81 | - | ||
| 82 | //A positive pitch angle moves head downwards (flying forward). |
- | |
| 83 | //A positive roll angle tilts left side downwards (flying left). |
- | |
| 84 | - | ||
| 85 | // If compass heading is 0 the head of the copter is in north direction. |
- | |
| 86 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
- | |
| 87 | // In case of a positive north deviation/velocity the |
- | |
| 88 | // copter should fly to south (negative pitch). |
- | |
| 89 | // In case of a positive east position deviation and a positive east velocity the |
- | |
| 90 | // copter should fly to west (positive roll). |
- | |
| 91 | - | ||
| 92 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
- | |
| 93 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
- | |
| 94 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
- | |
| 95 | - | ||
| 96 | // rotation transformation to compass heading to match any copter orientation |
- | |
| 97 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
- | |
| 98 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
- | |
| 99 | - | ||
| 100 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
- | |
| 101 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
- | |
| 102 | - | ||
| 103 | // limit GPS controls |
- | |
| 104 | #define GPS_CTRL_LIMIT 35 |
- | |
| 105 | if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
- | |
| 106 | if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
- | |
| 107 | if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
- | |
| 108 | if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
- | |
| 109 | } |
- | |
| 110 | else // MK controlled by user |
- | |
| 111 | { |
77 | case TSK_IDLE: |
| 112 | // update hold point to current gps position |
78 | // update hold point to current gps position |
| 113 | GPSHoldPoint.Northing = GPSInfo.utmnorth; |
79 | HoldPosition.Northing = GPSInfo.utmnorth; |
| 114 | GPSHoldPoint.Easting = GPSInfo.utmeast; |
80 | HoldPosition.Easting = GPSInfo.utmeast; |
| 115 | GPSHoldPoint.Status = VALID; |
81 | HoldPosition.Status = VALID; |
| 116 | // disable gps control |
82 | // disable gps control |
| 117 | GPS_Pitch = 0; |
83 | GPS_Pitch = 0; |
| - | 84 | GPS_Roll = 0; |
|
| - | 85 | break; // eof TSK_IDLE |
|
| - | 86 | case TSK_HOLD: |
|
| - | 87 | // if sticks are centered and hold position is valid enable position hold control |
|
| - | 88 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
|
| - | 89 | { |
|
| - | 90 | // Calculate deviation from hold position |
|
| - | 91 | GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
|
| - | 92 | GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
|
| - | 93 | DebugOut.Analog[12] = GPSPosDev_North; |
|
| - | 94 | DebugOut.Analog[13] = GPSPosDev_East; |
|
| - | 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 | ||
| - | 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 | } |
|
| - | 136 | } |
|
| - | 137 | else // MK controlled by user |
|
| - | 138 | { |
|
| - | 139 | // update hold point to current gps position |
|
| - | 140 | HoldPosition.Northing = GPSInfo.utmnorth; |
|
| - | 141 | HoldPosition.Easting = GPSInfo.utmeast; |
|
| - | 142 | HoldPosition.Status = VALID; |
|
| - | 143 | // disable gps control |
|
| - | 144 | GPS_Pitch = 0; |
|
| 118 | GPS_Roll = 0; |
145 | GPS_Roll = 0; |
| - | 146 | } |
|
| - | 147 | break; // eof TSK_HOLD |
|
| - | 148 | default: // unhandled task |
|
| - | 149 | GPS_Task = TSK_IDLE; |
|
| - | 150 | GPS_Pitch = 0; |
|
| - | 151 | GPS_Roll = 0; |
|
| - | 152 | 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; |
|
| 119 | } |
160 | if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
| - | 161 | } // eof 3D-FIX |
|
| 120 | } // eof 3D-FIX |
162 | |
| 121 | else // no 3D-SATFIX |
163 | else // no 3D-SATFIX |
| 122 | { // diable gps control |
164 | { // disable gps control |
| 123 | GPS_Pitch = 0; |
165 | GPS_Pitch = 0; |
| 124 | GPS_Roll = 0; |
166 | GPS_Roll = 0; |
| 125 | BeepTime = 50; |
167 | if(GPS_Task != TSK_IDLE) BeepTime = 50; |
| - | 168 | } |
|
| 126 | } |
169 | |
| 127 | // set current data as processed to avoid further calculations on the same gps data |
170 | // set current data as processed to avoid further calculations on the same gps data |
| 128 | GPSInfo.status = PROCESSED; |
171 | GPSInfo.status = PROCESSED; |
| 129 | break; |
172 | break; |
| 130 | } // eof GPSInfo.status |
- | |
| - | 173 | } // eof GPSInfo.status |
|
| 131 | } |
174 | DebugOut.Analog[14] = GPS_Pitch; |
| 132 | return; |
175 | DebugOut.Analog[15] = GPS_Roll; |