| 81,8 → 81,10 |
|---|
| GPS_Roll = 0; |
| } |
| |
| // calculates the GPS control sticks values from the deviation to target position |
| void GPS_PDController(GPS_Pos_t *TargetPos) |
| // calculates the GPS control stick values from the deviation to target position |
| // if the pointer to the target positin is NULL or is the target position invalid |
| // then the P part of the controller is deactivated. |
| void GPS_PDController(GPS_Pos_t *pTargetPos) |
| { |
| int32_t coscompass, sincompass; |
| int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| 90,35 → 92,55 |
|---|
| int32_t PD_North = 0, PD_East = 0; |
| static int32_t cos_target_latitude = 1; |
| |
| // if GPS data and Compass are ok |
| if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) ) |
| { |
| |
| if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
| { |
| // calculate position deviation from latitude and longitude differences |
| GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
| GPSPosDev_East = (GPSInfo.longitude - TargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
| // recalculate the target latitude projection only if the target data are updated |
| // to save time |
| if (TargetPos->Status != PROCESSED) |
| if(pTargetPos != NULL) // if there is a target position |
| { |
| cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L)); |
| TargetPos->Status = PROCESSED; |
| } |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East /= 8192; |
| if(pTargetPos->Status != INVALID) // and the position data are valid |
| { // calculate position deviation from latitude and longitude differences |
| GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
| GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
| // recalculate the target latitude projection only if the target data are updated |
| // to save time |
| if (pTargetPos->Status != PROCESSED) |
| { |
| cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
| pTargetPos->Status = PROCESSED; |
| } |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East /= 8192; |
| |
| DebugOut.Analog[12] = GPSPosDev_North; |
| DebugOut.Analog[13] = GPSPosDev_East; |
| DebugOut.Analog[12] = GPSPosDev_North; |
| DebugOut.Analog[13] = GPSPosDev_East; |
| //DebugOut.Analog[12] = GPSInfo.velnorth; |
| //DebugOut.Analog[13] = GPSInfo.veleast; |
| |
| //DebugOut.Analog[12] = GPSInfo.velnorth; |
| //DebugOut.Analog[13] = GPSInfo.veleast; |
| //Calculate P-components of the controller (negative sign for compensation) |
| P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| } |
| else // not valid target position available |
| { |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| } |
| } |
| else // not target position available |
| { |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| } |
| |
| //Calculate PD-components of the controller (negative sign for compensation) |
| P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| |
| // limit D-Part if position deviation is significant of the target position |
| // this accelerates flying to the target position |
| if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) ) |
| { |
| if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT; |
| 126,7 → 148,6 |
|---|
| if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT; |
| else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT; |
| } |
| |
| // PD-controller |
| PD_North = P_North + D_North; |
| PD_East = P_East + D_East; |
| 133,9 → 154,8 |
|---|
| |
| // GPS to pitch and roll settings |
| |
| //A positive pitch angle moves head downwards (flying forward). |
| //A positive roll angle tilts left side downwards (flying left). |
| |
| // A positive pitch 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. |
| // In case of a positive north deviation/velocity the |
| 142,23 → 162,15 |
|---|
| // copter should fly to south (negative pitch). |
| // 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 |
| // 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. |
| |
| // rotation transformation to compass heading to match any copter orientation |
| if (CompassHeading < 0) // no valid compass data |
| { // disable GPS |
| GPS_Neutral(); |
| } |
| else |
| { |
| coscompass = (int32_t)c_cos_8192(CompassHeading); |
| sincompass = (int32_t)c_sin_8192(CompassHeading); |
| GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
| GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
| } |
| coscompass = (int32_t)c_cos_8192(CompassHeading); |
| sincompass = (int32_t)c_sin_8192(CompassHeading); |
| GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
| GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
| |
| // limit GPS controls |
| if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
| else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
| 165,10 → 177,9 |
|---|
| if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
| else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
| } |
| else // invalid input data |
| else // invalid GPS data |
| { |
| BeepTime = 50; |
| GPS_Neutral(); |
| GPS_Neutral(); // do nothing |
| } |
| } |
| |
| 176,6 → 187,7 |
|---|
| void GPS_Main(uint8_t ctrl) |
| { |
| static uint8_t GPS_Task = TSK_IDLE; |
| static uint8_t GPS_P_Delay = 0; |
| int16_t satbeep; |
| |
| // ctrl enables the gps feature |
| 222,7 → 234,13 |
|---|
| // if sticks are centered (no manual control) |
| if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
| { |
| GPS_PDController(&HoldPosition); |
| if(GPS_P_Delay<7) |
| { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| GPS_P_Delay++; |
| GPS_SetHoldPosition(); // update hold point to current gps position |
| GPS_PDController(NULL); // activates only the D-Part |
| } |
| else GPS_PDController(&HoldPosition);// activates the P&D-Part |
| } |
| else // MK controlled by user |
| { |
| 230,6 → 248,7 |
|---|
| GPS_SetHoldPosition(); |
| // disable gps control |
| GPS_Neutral(); |
| GPS_P_Delay = 0; |
| } |
| } |
| else // invalid Hold Position |