Rev 726 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 726 | Rev 727 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
| 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" |
|
| Line 5... | Line 6... | ||
| 5 | #include "mymath.h" |
6 | #include "timer0.h" |
| 6 | 7 | ||
| Line 7... | Line 8... | ||
| 7 | int16_t GPS_Pitch = 0; |
8 | int16_t GPS_Pitch = 0; |
| 8 | int16_t GPS_Roll = 0; |
9 | int16_t GPS_Roll = 0; |
| Line 9... | Line 10... | ||
| 9 | 10 | ||
| 10 | uint8_t GPS_P_Factor = 0; |
11 | int32_t GPS_P_Factor = 0; |
| 11 | uint8_t GPS_D_Factor = 0; |
12 | int32_t GPS_D_Factor = 0; |
| 12 | 13 | ||
| Line 18... | Line 19... | ||
| 18 | 19 | ||
| Line 19... | Line 20... | ||
| 19 | } GPS_Pos_t; |
20 | } GPS_Pos_t; |
| 20 | 21 | ||
| 21 | // GPS coordinates for hold position |
- | |
| 22 | GPS_Pos_t GPSHoldPoint = {0,0, INVALID}; |
- | |
| Line 23... | Line 22... | ||
| 23 | // GPS coordinates for flying home |
22 | // GPS coordinates for hold position |
| Line 41... | Line 40... | ||
| 41 | switch(GPSInfo.status) |
40 | switch(GPSInfo.status) |
| 42 | { |
41 | { |
| 43 | case INVALID: // invalid gps data |
42 | case INVALID: // invalid gps data |
| 44 | GPS_Pitch = 0; |
43 | GPS_Pitch = 0; |
| 45 | GPS_Roll = 0; |
44 | GPS_Roll = 0; |
| - | 45 | BeepTime = 50; |
|
| 46 | break; |
46 | break; |
| 47 | case PROCESSED: // if gps data are already processed |
47 | case PROCESSED: // if gps data are already processed |
| 48 | // downcount timeout |
48 | // downcount timeout |
| 49 | if(GPSTimeout) GPSTimeout--; |
49 | if(GPSTimeout) GPSTimeout--; |
| 50 | // if no new data arrived within timeout set current data invalid |
50 | // if no new data arrived within timeout set current data invalid |
| Line 58... | Line 58... | ||
| 58 | break; |
58 | break; |
| 59 | case VALID: // new valid data from gps device |
59 | case VALID: // new valid data from gps device |
| 60 | // if the gps data quality is sufficient |
60 | // if the gps data quality is sufficient |
| 61 | if (GPSInfo.satfix == SATFIX_3D) |
61 | if (GPSInfo.satfix == SATFIX_3D) |
| 62 | { |
62 | { |
| 63 | // if sticks are centered and hold position is valid enable position hold |
63 | // if sticks are centered and hold position is valid enable position hold control |
| 64 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID)) |
64 | if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID)) |
| 65 | { |
65 | { |
| 66 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
- | |
| 67 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
- | |
| 68 | - | ||
| 69 | // Calculate deviation from hold position |
66 | // Calculate deviation from hold position |
| 70 | GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing; |
67 | GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing; |
| 71 | GPSPosDev_East = GPSInfo.utmeast - GPSHoldPoint.Easting; |
68 | GPSPosDev_East = GPSInfo.utmeast - GPSHoldPoint.Easting; |
| Line 72... | Line 69... | ||
| 72 | 69 | ||
| 73 | //Calculate PD-components of the controller |
70 | //Calculate PD-components of the controller (negative sign for compensation) |
| 74 | P_North = (GPS_P_Factor * GPSPosDev_North)/2048; |
71 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| 75 | D_North = (GPS_D_Factor * GPSInfo.velnorth)/255; |
72 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| 76 | P_East = (GPS_P_Factor * GPSPosDev_East)/2048; |
73 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| Line 77... | Line 74... | ||
| 77 | D_East = (GPS_D_Factor * GPSInfo.veleast)/255; |
74 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| 78 | 75 | ||
| 79 | // PD-controller |
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 |
|
| Line 80... | Line -... | ||
| 80 | PD_North = (-P_North + D_North); |
- | |
| 81 | PD_East = (P_East - D_East); |
97 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
| 82 | 98 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
|
| Line 83... | Line 99... | ||
| 83 | // GPS to pitch and roll |
99 | |
| - | 100 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
|
| 84 | GPS_Pitch = (int16_t)((coscompass * PD_North - sincompass * PD_East) / 8192); |
101 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
| 85 | GPS_Roll = (int16_t)((sincompass * PD_North + coscompass * PD_East) / 8192); |
102 | |
| 86 | 103 | // limit GPS controls |
|
| 87 | // limit GPS controls |
104 | #define GPS_CTRL_LIMIT 35 |
| 88 | if (GPS_Pitch > 35) GPS_Pitch = 35; |
105 | if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
| 89 | if (GPS_Pitch < -35) GPS_Pitch = -35; |
106 | if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
| 90 | if (GPS_Roll > 35) GPS_Roll = 35; |
107 | if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
| 91 | if (GPS_Roll < -35) GPS_Roll = -35; |
108 | if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
| 92 | } |
109 | } |
| Line 103... | Line 120... | ||
| 103 | } // eof 3D-FIX |
120 | } // eof 3D-FIX |
| 104 | else // no 3D-SATFIX |
121 | else // no 3D-SATFIX |
| 105 | { // diable gps control |
122 | { // diable gps control |
| 106 | GPS_Pitch = 0; |
123 | GPS_Pitch = 0; |
| 107 | GPS_Roll = 0; |
124 | GPS_Roll = 0; |
| - | 125 | BeepTime = 50; |
|
| 108 | } |
126 | } |
| 109 | // set current data as processed to avoid further calculations on the same gps data |
127 | // set current data as processed to avoid further calculations on the same gps data |
| 110 | GPSInfo.status = PROCESSED; |
128 | GPSInfo.status = PROCESSED; |
| 111 | break; |
129 | break; |
| 112 | } // eof GPSInfo.status |
130 | } // eof GPSInfo.status |