Subversion Repositories FlightCtrl

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

#include <inttypes.h>

#include <stdlib.h>

#include <stddef.h>

#include "ubx.h"

#include "configuration.h"

#include "controlMixer.h"

#include "output.h"

#include "isqrt.h"

#include "attitude.h"

#include "dongfangMath.h"

#include "attitude.h"

typedef enum {

  NAVI_FLIGHT_MODE_UNDEF,

  NAVI_FLIGHT_MODE_FREE,

  NAVI_FLIGHT_MODE_AID,

  NAVI_FLIGHT_MODE_HOME,

} FlightMode_t;

typedef enum {

  NAVI_STATUS_FREEFLIGHT=0,

  NAVI_STATUS_INVALID_GPS=1,

  NAVI_STATUS_BAD_GPS_SIGNAL=2,

  NAVI_STATUS_MANUAL_OVERRIDE=5,

  NAVI_STATUS_POSITION_HOLD=7,

  NAVI_STATUS_RTH=8,

  NAVI_STATUS_HOLD_POSITION_INVALID=9,

  NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10,

  NAVI_STATUS_RTH_POSITION_INVALID=11,

  NAVI_STATUS_GPS_TIMEOUT=12

} NaviStatus_t;

#define GPS_POSINTEGRAL_LIMIT 32000

#define LOG_NAVI_STICK_GAIN 3

#define GPS_P_LIMIT                     100

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 = NAVI_FLIGHT_MODE_UNDEF;

int16_t naviSticks[2] = {0,0};

uint8_t naviStatus;

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

void navi_updateFlightMode(void) {

  static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;

  if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {

    flightMode = NAVI_FLIGHT_MODE_FREE;

  } else {

    if (dynamicParams.naviMode < 50)

      flightMode = NAVI_FLIGHT_MODE_FREE;

    else if (dynamicParams.naviMode < 180)

      flightMode = NAVI_FLIGHT_MODE_AID;

    else

      flightMode = NAVI_FLIGHT_MODE_HOME;

  }

  if (flightMode != flightModeOld) {

    beep(100);

    flightModeOld = flightMode;

  }

}

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

// This function defines a good GPS signal condition

uint8_t navi_isGPSSignalOK(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 navi_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 navi_isManuallyControlled(int16_t* PRTY) {

  debugOut.analog[26] = PRTY[CONTROL_PITCH];

  debugOut.analog[27] = PRTY[CONTROL_ROLL];

  if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold

      && abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)

    return 0;

  else

    return 1;

}

// set given position to current gps position

uint8_t navi_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {

  if (pGPSPos == NULL)

    return 0; // bad pointer

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

    pGPSPos->longitude = GPSInfo.longitude;

    pGPSPos->latitude = GPSInfo.latitude;

    pGPSPos->altitude = GPSInfo.altitude;

    pGPSPos->status = NEWDATA;

    return 1;

  } else { // bad GPS signal condition

    pGPSPos->status = INVALID;

    return 0;

  }

}

// clear position

uint8_t navi_clearPosition(GPS_Pos_t * pGPSPos) {

  if (pGPSPos == NULL)

    return 0; // bad pointer

  else {

    pGPSPos->longitude = 0;

    pGPSPos->latitude = 0;

    pGPSPos->altitude = 0;

    pGPSPos->status = INVALID;

  }

  return 1;

}

void navi_setNeutral(void) {

  naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0;

}

// 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 navi_PIDController(GPS_Pos_t *pTargetPos) {

  static int32_t PID_Pitch, 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 (navi_isGPSSignalOK() && (magneticHeading >= 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 = cos_360(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 >>= LOG_MATH_UNIT_FACTOR;

      } 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) >> 9;

    D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;

    // P-Part

    P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) >> 11;

    P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) >> 11;

    // I-Part

    I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North) >> 13;

    I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13;

    // combine P & I

    PID_North = P_North + I_North;

    PID_East = P_East + I_East;

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

      // within limit

      GPSPosDevIntegral_North += GPSPosDev_North >> 4;

      GPSPosDevIntegral_East += GPSPosDev_East >> 4;

      navi_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 = -cos_360(heading / GYRO_DEG_FACTOR_YAW);

    sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);

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

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

    // limit resulting GPS control vector

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

    naviSticks[CONTROL_PITCH] = PID_Pitch;

    naviSticks[CONTROL_ROLL] = PID_Roll;

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

    // reset error integral

    navi_setNeutral();

    GPSPosDevIntegral_North = 0;

    GPSPosDevIntegral_East = 0;

  }

}

void navigation_periodicTaskAndPRTY(int16_t* PRTY) {

  static uint8_t GPS_P_Delay = 0;

  static uint16_t beep_rythm = 0;

  navi_updateFlightMode();

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

  if (MKFlags & MKFLAG_CALIBRATE) {

    MKFlags &= ~(MKFLAG_CALIBRATE);

    if (navi_writeCurrPositionTo(&homePosition)) {

        // shift north to simulate an offset.

        // homePosition.latitude += 10000L;

        beep(500);

      }

    }

  switch (GPSInfo.status) {

  case INVALID: // invalid gps data

    navi_setNeutral();

    naviStatus = NAVI_STATUS_INVALID_GPS;

    if (flightMode != NAVI_FLIGHT_MODE_FREE) {

      beep(1); // 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 {

      navi_setNeutral();

      GPSInfo.status = INVALID;

      naviStatus = NAVI_STATUS_GPS_TIMEOUT;

    }

    break;

  case NEWDATA: // new valid data from gps device

    // if the gps data quality is good

    beep_rythm++;

    if (navi_isGPSSignalOK()) {

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

      case NAVI_FLIGHT_MODE_FREE:

        // update hold position to current gps position

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

        // disable gps control

        navi_setNeutral();

        naviStatus = NAVI_STATUS_FREEFLIGHT;

        break;

      case NAVI_FLIGHT_MODE_AID:

        if (holdPosition.status != INVALID) {

          if (navi_isManuallyControlled(PRTY)) { // MK controlled by user

            // update hold point to current gps position

            navi_writeCurrPositionTo(&holdPosition);

            // disable gps control

            navi_setNeutral();

            GPS_P_Delay = 0;

            naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;

          } else { // GPS control active

            if (GPS_P_Delay < 7) {

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

              GPS_P_Delay++;

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

              navi_PIDController(NULL); // activates only the D-Part

              naviStatus = NAVI_STATUS_POSITION_HOLD;

            } else {

              navi_PIDController(&holdPosition); // activates the P&D-Part

              naviStatus = NAVI_STATUS_POSITION_HOLD;

            }

          }

        } else { // invalid Hold Position

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

          navi_writeCurrPositionTo(&holdPosition);

          navi_setNeutral();

          naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID;

        }

        break;

      case NAVI_FLIGHT_MODE_HOME:

        if (homePosition.status != INVALID) {

          // update hold point to current gps position

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

          navi_writeCurrPositionTo(&holdPosition);

          if (navi_isManuallyControlled(PRTY)) { // MK controlled by user

            navi_setNeutral();

            naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;

          } else {// GPS control active

            navi_PIDController(&homePosition);

          }

          naviStatus = NAVI_STATUS_RTH;

        } 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 (navi_isManuallyControlled(PRTY)) {

              // MK controlled by user

              navi_setNeutral();

              naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;

            } else {

              // GPS control active

              navi_PIDController(&holdPosition);

              naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD;

            }

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

            navi_writeCurrPositionTo(&holdPosition);

            naviStatus = NAVI_STATUS_RTH_POSITION_INVALID;

            navi_setNeutral();

          }

        }

        break; // eof TSK_HOME

      default: // unhandled task

        navi_setNeutral();

        break; // eof default

      } // eof switch GPS_Task

    } // eof gps data quality is good

    else { // gps data quality is bad

      // disable gps control

      navi_setNeutral();

      naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;

      if (flightMode != NAVI_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

  PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];

  PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];

  debugOut.analog[16] = flightMode;

  debugOut.analog[17] = naviStatus;

  debugOut.analog[18] = naviSticks[CONTROL_PITCH];

  debugOut.analog[19] = naviSticks[CONTROL_ROLL];

}