| 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 |