Rev 762 | Rev 767 | 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(); |