Rev 907 | Rev 935 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 907 | Rev 911 | ||
---|---|---|---|
Line 36... | Line 36... | ||
36 | GPS_Pos_t HomePosition = {0,0,0,INVALID}; |
36 | GPS_Pos_t HomePosition = {0,0,0,INVALID}; |
Line 37... | Line 37... | ||
37 | 37 | ||
Line 38... | Line 38... | ||
38 | 38 | ||
39 | // --------------------------------------------------------------------------------- |
39 | // --------------------------------------------------------------------------------- |
40 | 40 | ||
41 | // checks pitch and roll sticks for manual control |
41 | // checks nick and roll sticks for manual control |
42 | uint8_t IsManualControlled(void) |
42 | uint8_t IsManualControlled(void) |
43 | { |
43 | { |
Line 44... | Line 44... | ||
44 | if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0; |
44 | if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0; |
45 | else return 1; |
45 | else return 1; |
Line 85... | Line 85... | ||
85 | } |
85 | } |
Line 86... | Line 86... | ||
86 | 86 | ||
87 | // disable GPS control sticks |
87 | // disable GPS control sticks |
88 | void GPS_Neutral(void) |
88 | void GPS_Neutral(void) |
89 | { |
89 | { |
90 | GPS_Pitch = 0; |
90 | GPS_Nick = 0; |
91 | GPS_Roll = 0; |
91 | GPS_Roll = 0; |
Line 92... | Line 92... | ||
92 | } |
92 | } |
93 | 93 | ||
94 | // calculates the GPS control stick values from the deviation to target position |
94 | // calculates the GPS control stick values from the deviation to target position |
95 | // if the pointer to the target positin is NULL or is the target position invalid |
95 | // if the pointer to the target positin is NULL or is the target position invalid |
96 | // then the P part of the controller is deactivated. |
96 | // then the P part of the controller is deactivated. |
97 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
97 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
98 | { |
98 | { |
99 | int32_t temp, temp1, PID_Pitch, PID_Roll; |
99 | int32_t temp, temp1, PID_Nick, PID_Roll; |
100 | int32_t coscompass, sincompass; |
100 | int32_t coscompass, sincompass; |
101 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
101 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
102 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
102 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
Line 194... | Line 194... | ||
194 | 194 | ||
195 | // combine PI- and D-Part |
195 | // combine PI- and D-Part |
196 | PID_North += D_North; |
196 | PID_North += D_North; |
Line 197... | Line 197... | ||
197 | PID_East += D_East; |
197 | PID_East += D_East; |
Line 198... | Line 198... | ||
198 | 198 | ||
199 | // GPS to pitch and roll settings |
199 | // GPS to nick and roll settings |
200 | 200 | ||
201 | // A positive pitch angle moves head downwards (flying forward). |
201 | // A positive nick angle moves head downwards (flying forward). |
202 | // A positive roll angle tilts left side downwards (flying left). |
202 | // A positive roll angle tilts left side downwards (flying left). |
203 | // If compass heading is 0 the head of the copter is in north direction. |
203 | // If compass heading is 0 the head of the copter is in north direction. |
204 | // A positive pitch angle will fly to north and a positive roll angle will fly to west. |
204 | // A positive nick angle will fly to north and a positive roll angle will fly to west. |
205 | // In case of a positive north deviation/velocity the |
205 | // In case of a positive north deviation/velocity the |
206 | // copter should fly to south (negative pitch). |
206 | // copter should fly to south (negative nick). |
207 | // In case of a positive east position deviation and a positive east velocity the |
207 | // In case of a positive east position deviation and a positive east velocity the |
208 | // copter should fly to west (positive roll). |
208 | // copter should fly to west (positive roll). |
Line 209... | Line 209... | ||
209 | // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
209 | // The influence of the GPS_Nick and GPS_Roll variable is contrarily to the stick values |
210 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
210 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
211 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
211 | // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
212 | 212 | ||
Line 213... | Line 213... | ||
213 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
213 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
214 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
214 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
215 | PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
215 | PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
216 | PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
216 | PID_Nick = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
217 | 217 | ||
218 | // limit resulting GPS control vector |
218 | // limit resulting GPS control vector |
219 | temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch); |
219 | temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Nick*PID_Nick); |
220 | if (temp > GPS_STICK_LIMIT) |
220 | if (temp > GPS_STICK_LIMIT) |
Line 221... | Line 221... | ||
221 | { |
221 | { |
222 | // normalize control vector components to the limit |
222 | // normalize control vector components to the limit |
Line 223... | Line 223... | ||
223 | PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp; |
223 | PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp; |
224 | PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/temp; |
224 | PID_Nick = (PID_Nick * GPS_STICK_LIMIT)/temp; |
225 | } |
225 | } |
226 | 226 |