Subversion Repositories FlightCtrl

Rev

Rev 907 | Show entire file | Ignore 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