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