Subversion Repositories FlightCtrl

  GPS_FLIGHT_MODE_AID,

  GPS_FLIGHT_MODE_AID,

  GPS_FLIGHT_MODE_HOME,

  GPS_FLIGHT_MODE_HOME,

} FlightMode_t;

} FlightMode_t;

#define GPS_STICK_LIMIT         400             // limit of gps stick control to avoid critical flight attitudes

#define LOG_NAVI_STICK_GAIN 2

#define GPS_P_LIMIT                     100

#define GPS_P_LIMIT                     100

typedef struct {

typedef struct {

        // calculate position deviation from latitude and longitude differences

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

        GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally

        // calculate latitude projection

        // calculate latitude projection

        GPSPosDev_East *= cos_target_latitude;

        GPSPosDev_East *= cos_target_latitude;

      } else { // no valid target position available

      } else { // no valid target position available

        // reset error

        // reset error

        GPSPosDev_North = 0;

        GPSPosDev_North = 0;

        GPSPosDev_East = 0;

        GPSPosDev_East = 0;

        // reset error integral

        // reset error integral

    // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.

    // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.

    coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);

    coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);

    PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2);

    PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);

    // limit resulting GPS control vector

    // limit resulting GPS control vector

    navi_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);

    navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);