38,10 → 38,10 |
|
// --------------------------------------------------------------------------------- |
|
// checks pitch and roll sticks for manual control |
// checks nick and roll sticks for manual control |
uint8_t IsManualControlled(void) |
{ |
if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0; |
if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0; |
else return 1; |
} |
|
87,7 → 87,7 |
// disable GPS control sticks |
void GPS_Neutral(void) |
{ |
GPS_Pitch = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
|
96,7 → 96,7 |
// then the P part of the controller is deactivated. |
void GPS_PIDController(GPS_Pos_t *pTargetPos) |
{ |
int32_t temp, temp1, PID_Pitch, PID_Roll; |
int32_t temp, temp1, PID_Nick, PID_Roll; |
int32_t coscompass, sincompass; |
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
196,36 → 196,36 |
PID_North += D_North; |
PID_East += D_East; |
|
// GPS to pitch and roll settings |
// GPS to nick and roll settings |
|
// A positive pitch angle moves head downwards (flying forward). |
// A positive nick angle moves head downwards (flying forward). |
// A positive roll angle tilts left side downwards (flying left). |
// If compass heading is 0 the head of the copter is in north direction. |
// A positive pitch angle will fly to north and a positive roll angle will fly to west. |
// A positive nick angle will fly to north and a positive roll angle will fly to west. |
// In case of a positive north deviation/velocity the |
// copter should fly to south (negative pitch). |
// copter should fly to south (negative nick). |
// In case of a positive east position deviation and a positive east velocity the |
// copter should fly to west (positive roll). |
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
// The influence of the GPS_Nick and GPS_Roll variable is contrarily to the stick values |
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
// GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
|
coscompass = (int32_t)c_cos_8192(CompassHeading); |
sincompass = (int32_t)c_sin_8192(CompassHeading); |
PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
PID_Nick = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
|
// limit resulting GPS control vector |
temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch); |
temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Nick*PID_Nick); |
if (temp > GPS_STICK_LIMIT) |
{ |
// normalize control vector components to the limit |
PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp; |
PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/temp; |
PID_Nick = (PID_Nick * GPS_STICK_LIMIT)/temp; |
} |
|
GPS_Roll = (int16_t)PID_Roll; |
GPS_Pitch = (int16_t)PID_Pitch; |
GPS_Nick = (int16_t)PID_Nick; |
|
} |
else // invalid GPS data or bad compass reading |