Rev 762 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 762 | Rev 764 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
| - | 2 | #include <stdlib.h> |
|
| 2 | #include "fc.h" |
3 | #include "fc.h" |
| 3 | #include "ubx.h" |
4 | #include "ubx.h" |
| 4 | #include "mymath.h" |
5 | #include "mymath.h" |
| 5 | #include "timer0.h" |
6 | #include "timer0.h" |
| 6 | #include "uart.h" |
7 | #include "uart.h" |
| Line 9... | Line 10... | ||
| 9 | #define TSK_HOLD 1 |
10 | #define TSK_HOLD 1 |
| 10 | #define TSK_HOME 2 |
11 | #define TSK_HOME 2 |
| Line 11... | Line 12... | ||
| 11 | 12 | ||
| 12 | #define GPS_STICK_SENSE 12 |
13 | #define GPS_STICK_SENSE 12 |
| - | 14 | #define GPS_STICK_LIMIT 45 |
|
| - | 15 | #define GPS_D_LIMIT_DIST 500 // for position deviations grater than 500 cm (5m) the D-Part of the PD-Controller is limited |
|
| Line 13... | Line 16... | ||
| 13 | #define GPS_STICK_LIMIT 45 |
16 | #define GPS_D_LIMIT 50 // PD-Controntroller D-Limit. |
| 14 | 17 | ||
| Line 15... | Line 18... | ||
| 15 | int16_t GPS_Pitch = 0; |
18 | int16_t GPS_Pitch = 0; |
| Line 76... | Line 79... | ||
| 76 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
79 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| 77 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
80 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| 78 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
81 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| 79 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
82 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| Line -... | Line 83... | ||
| - | 83 | ||
| - | 84 | if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) ) |
|
| - | 85 | { |
|
| - | 86 | if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT; |
|
| - | 87 | else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT; |
|
| - | 88 | if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT; |
|
| - | 89 | else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT; |
|
| - | 90 | } |
|
| 80 | 91 | ||
| 81 | // PD-controller |
92 | // PD-controller |
| 82 | PD_North = P_North + D_North; |
93 | PD_North = P_North + D_North; |
| Line 83... | Line 94... | ||
| 83 | PD_East = P_East + D_East; |
94 | PD_East = P_East + D_East; |
| Line 110... | Line 121... | ||
| 110 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
121 | GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
| 111 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
122 | GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
| 112 | } |
123 | } |
| 113 | // limit GPS controls |
124 | // limit GPS controls |
| 114 | if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
125 | if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
| 115 | if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
126 | else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
| 116 | if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
127 | if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
| 117 | if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
128 | else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
| 118 | } |
129 | } |
| 119 | else // invalid input data |
130 | else // invalid input data |
| 120 | { |
131 | { |
| 121 | BeepTime = 50; |
132 | BeepTime = 50; |
| 122 | GPS_Neutral(); |
133 | GPS_Neutral(); |