Subversion Repositories FlightCtrl

Rev

Rev 909 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 909 Rev 916
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