Subversion Repositories FlightCtrl

// Navigation with a GPS directly attached to the FC's UART1.

#include <inttypes.h>

#include <stdlib.h>

#include <stddef.h>

//#include "mymath.h"

//#include "timer0.h"

//#include "uart1.h"

//#include "rc.h"

//#include "eeprom.h"

#include "ubx.h"

#include "configuration.h"

#include "controlMixer.h"

#include "output.h"

#include "isqrt.h"

#include "attitude.h"

#include "dongfangMath.h"

typedef enum {

  GPS_FLIGHT_MODE_UNDEF,

  GPS_FLIGHT_MODE_FREE,

  GPS_FLIGHT_MODE_AID,

  GPS_FLIGHT_MODE_HOME,

} FlightMode_t;

#define GPS_POSINTEGRAL_LIMIT 32000

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

#define GPS_P_LIMIT                     25

typedef struct {

  int32_t longitude;

  int32_t latitude;

  int32_t altitude;

  Status_t status;

} GPS_Pos_t;

// GPS coordinates for hold position

GPS_Pos_t holdPosition = { 0, 0, 0, INVALID };

// GPS coordinates for home position

GPS_Pos_t homePosition = { 0, 0, 0, INVALID };

// the current flight mode

FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;

// ---------------------------------------------------------------------------------

void GPS_updateFlightMode(void) {

  static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;

  if (controlMixer_getSignalQuality() <= SIGNAL_BAD

      || MKFlags & MKFLAG_EMERGENCY_FLIGHT) {

    flightMode = GPS_FLIGHT_MODE_FREE;

  } else {

    if (dynamicParams.directGPSMode < 50)

      flightMode = GPS_FLIGHT_MODE_AID;

    else if (dynamicParams.directGPSMode < 180)

      flightMode = GPS_FLIGHT_MODE_FREE;

    else

      flightMode = GPS_FLIGHT_MODE_HOME;

  }

  if (flightMode != flightModeOld) {

    beep(100);

    flightModeOld = flightMode;

  }

}

// ---------------------------------------------------------------------------------

// This function defines a good GPS signal condition

uint8_t GPS_isSignalOK(void) {

  static uint8_t GPSFix = 0;

  if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)

      && (GPSInfo.flags & FLAG_GPSFIXOK)

      && ((GPSInfo.satnum >= staticParams.GPSMininumSatellites) || GPSFix)) {

    GPSFix = 1;

    return 1;

  } else

    return (0);

}

// ---------------------------------------------------------------------------------

// rescale xy-vector length to  limit

uint8_t GPS_limitXY(int32_t *x, int32_t *y, int32_t limit) {

  int32_t len;

  len = isqrt32(*x * *x + *y * *y);

  if (len > limit) {

    // normalize control vector components to the limit

    *x = (*x * limit) / len;

    *y = (*y * limit) / len;

    return 1;

  }

  return 0;

}

// checks nick and roll sticks for manual control

uint8_t GPS_isManuallyControlled(int16_t* naviSticks) {

  if (naviSticks[CONTROL_PITCH] < staticParams.naviStickThreshold

      && naviSticks[CONTROL_ROLL] < staticParams.naviStickThreshold)

    return 0;

  else

    return 1;

}

// set given position to current gps position

uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) {

  uint8_t retval = 0;

  if (pGPSPos == NULL)

    return (retval); // bad pointer

  if (GPS_isSignalOK()) { // is GPS signal condition is fine

    pGPSPos->longitude = GPSInfo.longitude;

    pGPSPos->latitude = GPSInfo.latitude;

    pGPSPos->altitude = GPSInfo.altitude;

    pGPSPos->status = NEWDATA;

    retval = 1;

  } else { // bad GPS signal condition

    pGPSPos->status = INVALID;

    retval = 0;

  }

  return (retval);

}

// clear position

uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) {

  uint8_t retval = 0;

  if (pGPSPos == NULL)

    return retval; // bad pointer

  else {

    pGPSPos->longitude = 0;

    pGPSPos->latitude = 0;

    pGPSPos->altitude = 0;

    pGPSPos->status = INVALID;

    retval = 1;

  }

  return (retval);

}

// 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_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) {

  static int32_t 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;

  int32_t PID_North = 0, PID_East = 0;

  static int32_t cos_target_latitude = 1;

  static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;

  static GPS_Pos_t *pLastTargetPos = 0;

  // if GPS data and Compass are ok

  if (GPS_isSignalOK() && (compassHeading >= 0)) {

    if (pTargetPos != NULL) // if there is a target position

        {

      if (pTargetPos->status != INVALID) // and the position data are valid

          {

        // if the target data are updated or the target pointer has changed

        if ((pTargetPos->status != PROCESSED)

            || (pTargetPos != pLastTargetPos)) {

          // reset error integral

          GPSPosDevIntegral_North = 0;

          GPSPosDevIntegral_East = 0;

          // recalculate latitude projection

          cos_target_latitude = int_cos((int16_t) (pTargetPos->latitude / 10000000L));

          // remember last target pointer

          pLastTargetPos = pTargetPos;

          // mark data as processed

          pTargetPos->status = PROCESSED;

        }

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

        // calculate latitude projection

        GPSPosDev_East *= cos_target_latitude;

        GPSPosDev_East >>= MATH_UNIT_FACTOR_LOG;

      } else { // no valid target position available

        // reset error

        GPSPosDev_North = 0;

        GPSPosDev_East = 0;

        // reset error integral

        GPSPosDevIntegral_North = 0;

        GPSPosDevIntegral_East = 0;

      }

    } else { // no target position available

      // reset error

      GPSPosDev_North = 0;

      GPSPosDev_East = 0;

      // reset error integral

      GPSPosDevIntegral_North = 0;

      GPSPosDevIntegral_East = 0;

    }

    //Calculate PID-components of the controller

    // D-Part

    D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) / 512;

    D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) / 512;

    // P-Part

    P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) / 2048;

    P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) / 2048;

    // I-Part

    I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North)

        / 8192;

    I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) / 8192;

    // combine P & I

    PID_North = P_North + I_North;

    PID_East = P_East + I_East;

    if (!GPS_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {

      GPSPosDevIntegral_North += GPSPosDev_North / 16;

      GPSPosDevIntegral_East += GPSPosDev_East / 16;

      GPS_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East,

          GPS_POSINTEGRAL_LIMIT);

    }

    // combine PI- and D-Part

    PID_North += D_North;

    PID_East += D_East;

    // scale combination with gain.

    // dongfang: Lets not do that. P I and D can be scaled instead.

    // PID_North = (PID_North * (int32_t) staticParams.NaviGpsGain) / 100;

    // PID_East = (PID_East * (int32_t) staticParams.NaviGpsGain) / 100;

    // GPS to nick and roll settings

    // 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 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 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 GPSStickNick and GPSStickRoll variable is contrarily to the stick values

    // in the flight.c. Therefore a positive north deviation/velocity should result in a positive

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

    coscompass = int_cos(yawGyroHeading / GYRO_DEG_FACTOR_YAW);

    sincompass = int_sin(yawGyroHeading / GYRO_DEG_FACTOR_YAW);

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

    PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG;

    // limit resulting GPS control vector

    GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);

    sticks[CONTROL_PITCH] += (int16_t) PID_Nick;

    sticks[CONTROL_ROLL] += (int16_t) PID_Roll;

  } else { // invalid GPS data or bad compass reading

    // reset error integral

    GPSPosDevIntegral_North = 0;

    GPSPosDevIntegral_East = 0;

  }

}

void navigation_periodicTask(int16_t* sticks) {

  static uint8_t GPS_P_Delay = 0;

  static uint16_t beep_rythm = 0;

  GPS_updateFlightMode();

  // store home position if start of flight flag is set

  if (MKFlags & MKFLAG_CALIBRATE) {

    if (GPS_setCurrPosition(&homePosition))

      beep(700);

  }

  switch (GPSInfo.status) {

  case INVALID: // invalid gps data

    if (flightMode != GPS_FLIGHT_MODE_FREE) {

      beep(100); // beep if signal is neccesary

    }

    break;

  case PROCESSED: // if gps data are already processed do nothing

    // downcount timeout

    if (GPSTimeout)

      GPSTimeout--;

    // if no new data arrived within timeout set current data invalid

    // and therefore disable GPS

    else {

      GPSInfo.status = INVALID;

    }

    break;

  case NEWDATA: // new valid data from gps device

    // if the gps data quality is good

    beep_rythm++;

    if (GPS_isSignalOK()) {

      switch (flightMode) { // check what's to do

      case GPS_FLIGHT_MODE_FREE:

        // update hold position to current gps position

        GPS_setCurrPosition(&holdPosition); // can get invalid if gps signal is bad

        // disable gps control

        break;

      case GPS_FLIGHT_MODE_AID:

        if (holdPosition.status != INVALID) {

          if (GPS_isManuallyControlled(sticks)) { // MK controlled by user

            // update hold point to current gps position

            GPS_setCurrPosition(&holdPosition);

            // disable gps control

            GPS_P_Delay = 0;

          } else { // GPS control active

            if (GPS_P_Delay < 7) {

              // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)

              GPS_P_Delay++;

              GPS_setCurrPosition(&holdPosition); // update hold point to current gps position

              GPS_PIDController(NULL, sticks); // activates only the D-Part

            } else

              GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part

          }

        } else // invalid Hold Position

        { // try to catch a valid hold position from gps data input

          GPS_setCurrPosition(&holdPosition);

        }

        break;

      case GPS_FLIGHT_MODE_HOME:

        if (homePosition.status != INVALID) {

          // update hold point to current gps position

          // to avoid a flight back if home comming is deactivated

          GPS_setCurrPosition(&holdPosition);

          if (GPS_isManuallyControlled(sticks)) // MK controlled by user

          {

          } else {// GPS control active

            GPS_PIDController(&homePosition, sticks);

          }

        } else {

          // bad home position

          beep(50); // signal invalid home position

          // try to hold at least the position as a fallback option

          if (holdPosition.status != INVALID) {

            if (GPS_isManuallyControlled(sticks)) {

              // MK controlled by user

            } else {

              // GPS control active

              GPS_PIDController(&holdPosition, sticks);

            }

          } else { // try to catch a valid hold position

            GPS_setCurrPosition(&holdPosition);

          }

        }

        break; // eof TSK_HOME

      default: // unhandled task

        break; // eof default

      } // eof switch GPS_Task

    } // eof gps data quality is good

    else // gps data quality is bad

    { // disable gps control

      if (flightMode != GPS_FLIGHT_MODE_FREE) {

        // beep if signal is not sufficient

        if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))

          beep(100);

        else if (GPSInfo.satnum < staticParams.GPSMininumSatellites

            && !(beep_rythm % 5))

          beep(10);

      }

    }

    // set current data as processed to avoid further calculations on the same gps data

    GPSInfo.status = PROCESSED;

    break;

  } // eof GPSInfo.status

}