WebSVN - FlightCtrl - Diff - Rev 2086 and 2087 - /branches/dongfang_FC


// 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;
 
  static uint16_t navi_testOscPrescaler = 0;
  static uint8_t  navi_testOscTimer = 0;
 
  navi_testOscPrescaler++;
  if (navi_testOscPrescaler == 488) {
          navi_testOscPrescaler = 0;
          navi_testOscTimer++;
          if (navi_testOscTimer == staticParams.naviTestOscPeriod) {
                  navi_testOscTimer = 0;
                  if (staticParams.naviTestOscAmplitude) {
                          holdPosition.status = NEWDATA;
                          holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L;
                  }
          } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) {
                  if (staticParams.naviTestOscAmplitude) {
                          holdPosition.status = NEWDATA;
                          holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L;
                  }
          }
  }
 
  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];
}
 

Rev 2086	Rev 2087
1	// Navigation with a GPS directly attached to the FC's UART1.	1	// Navigation with a GPS directly attached to the FC's UART1.
2		2
3	#include <inttypes.h>	3	#include <inttypes.h>
4	#include <stdlib.h>	4	#include <stdlib.h>
5	#include <stddef.h>	5	#include <stddef.h>
6	#include "ubx.h"	6	#include "ubx.h"
7	#include "configuration.h"	7	#include "configuration.h"
8	#include "controlMixer.h"	8	#include "controlMixer.h"
9	#include "output.h"	9	#include "output.h"
10	#include "isqrt.h"	10	#include "isqrt.h"
11	#include "attitude.h"	11	#include "attitude.h"
12	#include "dongfangMath.h"	12	#include "dongfangMath.h"
13	#include "attitude.h"	13	#include "attitude.h"
14		14
15	typedef enum {	15	typedef enum {
16	NAVI_FLIGHT_MODE_UNDEF,	16	NAVI_FLIGHT_MODE_UNDEF,
17	NAVI_FLIGHT_MODE_FREE,	17	NAVI_FLIGHT_MODE_FREE,
18	NAVI_FLIGHT_MODE_AID,	18	NAVI_FLIGHT_MODE_AID,
19	NAVI_FLIGHT_MODE_HOME,	19	NAVI_FLIGHT_MODE_HOME,
20	} FlightMode_t;	20	} FlightMode_t;
21		21
22	typedef enum {	22	typedef enum {
23	NAVI_STATUS_FREEFLIGHT=0,	23	NAVI_STATUS_FREEFLIGHT=0,
24	NAVI_STATUS_INVALID_GPS=1,	24	NAVI_STATUS_INVALID_GPS=1,
25	NAVI_STATUS_BAD_GPS_SIGNAL=2,	25	NAVI_STATUS_BAD_GPS_SIGNAL=2,
26	NAVI_STATUS_MANUAL_OVERRIDE=5,	26	NAVI_STATUS_MANUAL_OVERRIDE=5,
27	NAVI_STATUS_POSITION_HOLD=7,	27	NAVI_STATUS_POSITION_HOLD=7,
28	NAVI_STATUS_RTH=8,	28	NAVI_STATUS_RTH=8,
29	NAVI_STATUS_HOLD_POSITION_INVALID=9,	29	NAVI_STATUS_HOLD_POSITION_INVALID=9,
30	NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10,	30	NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10,
31	NAVI_STATUS_RTH_POSITION_INVALID=11,	31	NAVI_STATUS_RTH_POSITION_INVALID=11,
32	NAVI_STATUS_GPS_TIMEOUT=12	32	NAVI_STATUS_GPS_TIMEOUT=12
33	} NaviStatus_t;	33	} NaviStatus_t;
34		34
35	#define GPS_POSINTEGRAL_LIMIT 32000	35	#define GPS_POSINTEGRAL_LIMIT 32000
36	#define LOG_NAVI_STICK_GAIN 3	36	#define LOG_NAVI_STICK_GAIN 3
37	#define GPS_P_LIMIT 100	37	#define GPS_P_LIMIT 100
38		38
39	typedef struct {	39	typedef struct {
40	int32_t longitude;	40	int32_t longitude;
41	int32_t latitude;	41	int32_t latitude;
42	int32_t altitude;	42	int32_t altitude;
43	Status_t status;	43	Status_t status;
44	} GPS_Pos_t;	44	} GPS_Pos_t;
45		45
46	// GPS coordinates for hold position	46	// GPS coordinates for hold position
47	GPS_Pos_t holdPosition = { 0, 0, 0, INVALID };	47	GPS_Pos_t holdPosition = { 0, 0, 0, INVALID };
48	// GPS coordinates for home position	48	// GPS coordinates for home position
49	GPS_Pos_t homePosition = { 0, 0, 0, INVALID };	49	GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
50	// the current flight mode	50	// the current flight mode
51	FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;	51	FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
52	int16_t naviSticks[2] = {0,0};	52	int16_t naviSticks[2] = {0,0};
53		53
54	uint8_t naviStatus;	54	uint8_t naviStatus;
55		55
56	// ---------------------------------------------------------------------------------	56	// ---------------------------------------------------------------------------------
57	void navi_updateFlightMode(void) {	57	void navi_updateFlightMode(void) {
58	static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;	58	static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
59		59
60	if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {	60	if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
61	flightMode = NAVI_FLIGHT_MODE_FREE;	61	flightMode = NAVI_FLIGHT_MODE_FREE;
62	} else {	62	} else {
63	if (dynamicParams.naviMode < 50)	63	if (dynamicParams.naviMode < 50)
64	flightMode = NAVI_FLIGHT_MODE_FREE;	64	flightMode = NAVI_FLIGHT_MODE_FREE;
65	else if (dynamicParams.naviMode < 180)	65	else if (dynamicParams.naviMode < 180)
66	flightMode = NAVI_FLIGHT_MODE_AID;	66	flightMode = NAVI_FLIGHT_MODE_AID;
67	else	67	else
68	flightMode = NAVI_FLIGHT_MODE_HOME;	68	flightMode = NAVI_FLIGHT_MODE_HOME;
69	}	69	}
70		70
71	if (flightMode != flightModeOld) {	71	if (flightMode != flightModeOld) {
72	beep(100);	72	beep(100);
73	flightModeOld = flightMode;	73	flightModeOld = flightMode;
74	}	74	}
75	}	75	}
76		76
77	// ---------------------------------------------------------------------------------	77	// ---------------------------------------------------------------------------------
78	// This function defines a good GPS signal condition	78	// This function defines a good GPS signal condition
79	uint8_t navi_isGPSSignalOK(void) {	79	uint8_t navi_isGPSSignalOK(void) {
80	static uint8_t GPSFix = 0;	80	static uint8_t GPSFix = 0;
81	if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)	81	if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)
82	&& (GPSInfo.flags & FLAG_GPSFIXOK)	82	&& (GPSInfo.flags & FLAG_GPSFIXOK)
83	&& ((GPSInfo.satnum >= staticParams.GPSMininumSatellites) \|\| GPSFix)) {	83	&& ((GPSInfo.satnum >= staticParams.GPSMininumSatellites) \|\| GPSFix)) {
84	GPSFix = 1;	84	GPSFix = 1;
85	return 1;	85	return 1;
86	} else	86	} else
87	return (0);	87	return (0);
88	}	88	}
89		89
90	// ---------------------------------------------------------------------------------	90	// ---------------------------------------------------------------------------------
91	// rescale xy-vector length to limit	91	// rescale xy-vector length to limit
92	uint8_t navi_limitXY(int32_t x, int32_t y, int32_t limit) {	92	uint8_t navi_limitXY(int32_t x, int32_t y, int32_t limit) {
93	int32_t len;	93	int32_t len;
94	len = isqrt32(x x + y * *y);	94	len = isqrt32(x x + y * *y);
95	if (len > limit) {	95	if (len > limit) {
96	// normalize control vector components to the limit	96	// normalize control vector components to the limit
97	x = (x * limit) / len;	97	x = (x * limit) / len;
98	y = (y * limit) / len;	98	y = (y * limit) / len;
99	return 1;	99	return 1;
100	}	100	}
101	return 0;	101	return 0;
102	}	102	}
103		103
104	// checks nick and roll sticks for manual control	104	// checks nick and roll sticks for manual control
105	uint8_t navi_isManuallyControlled(int16_t* PRTY) {	105	uint8_t navi_isManuallyControlled(int16_t* PRTY) {
106	debugOut.analog[26] = PRTY[CONTROL_PITCH];	106	debugOut.analog[26] = PRTY[CONTROL_PITCH];
107	debugOut.analog[27] = PRTY[CONTROL_ROLL];	107	debugOut.analog[27] = PRTY[CONTROL_ROLL];
108	if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold	108	if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
109	&& abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)	109	&& abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold)
110	return 0;	110	return 0;
111	else	111	else
112	return 1;	112	return 1;
113	}	113	}
114		114
115	// set given position to current gps position	115	// set given position to current gps position
116	uint8_t navi_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {	116	uint8_t navi_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
117	if (pGPSPos == NULL)	117	if (pGPSPos == NULL)
118	return 0; // bad pointer	118	return 0; // bad pointer
119		119
120	if (navi_isGPSSignalOK()) { // is GPS signal condition is fine	120	if (navi_isGPSSignalOK()) { // is GPS signal condition is fine
121	pGPSPos->longitude = GPSInfo.longitude;	121	pGPSPos->longitude = GPSInfo.longitude;
122	pGPSPos->latitude = GPSInfo.latitude;	122	pGPSPos->latitude = GPSInfo.latitude;
123	pGPSPos->altitude = GPSInfo.altitude;	123	pGPSPos->altitude = GPSInfo.altitude;
124	pGPSPos->status = NEWDATA;	124	pGPSPos->status = NEWDATA;
125	return 1;	125	return 1;
126	} else { // bad GPS signal condition	126	} else { // bad GPS signal condition
127	pGPSPos->status = INVALID;	127	pGPSPos->status = INVALID;
128	return 0;	128	return 0;
129	}	129	}
130	}	130	}
131		131
132	// clear position	132	// clear position
133	uint8_t navi_clearPosition(GPS_Pos_t * pGPSPos) {	133	uint8_t navi_clearPosition(GPS_Pos_t * pGPSPos) {
134	if (pGPSPos == NULL)	134	if (pGPSPos == NULL)
135	return 0; // bad pointer	135	return 0; // bad pointer
136	else {	136	else {
137	pGPSPos->longitude = 0;	137	pGPSPos->longitude = 0;
138	pGPSPos->latitude = 0;	138	pGPSPos->latitude = 0;
139	pGPSPos->altitude = 0;	139	pGPSPos->altitude = 0;
140	pGPSPos->status = INVALID;	140	pGPSPos->status = INVALID;
141	}	141	}
142	return 1;	142	return 1;
143	}	143	}
144		144
145	void navi_setNeutral(void) {	145	void navi_setNeutral(void) {
146	naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0;	146	naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0;
147	}	147	}
148		148
149	// calculates the GPS control stick values from the deviation to target position	149	// calculates the GPS control stick values from the deviation to target position
150	// if the pointer to the target positin is NULL or is the target position invalid	150	// if the pointer to the target positin is NULL or is the target position invalid
151	// then the P part of the controller is deactivated.	151	// then the P part of the controller is deactivated.
152	void navi_PIDController(GPS_Pos_t *pTargetPos) {	152	void navi_PIDController(GPS_Pos_t *pTargetPos) {
153	static int32_t PID_Pitch, PID_Roll;	153	static int32_t PID_Pitch, PID_Roll;
154	int32_t coscompass, sincompass;	154	int32_t coscompass, sincompass;
155	int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm	155	int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
156	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;	156	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
157	int32_t PID_North = 0, PID_East = 0;	157	int32_t PID_North = 0, PID_East = 0;
158	static int32_t cos_target_latitude = 1;	158	static int32_t cos_target_latitude = 1;
159	static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;	159	static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
160	static GPS_Pos_t *pLastTargetPos = 0;	160	static GPS_Pos_t *pLastTargetPos = 0;
161		161
162	// if GPS data and Compass are ok	162	// if GPS data and Compass are ok
163	if (navi_isGPSSignalOK() && (magneticHeading >= 0)) {	163	if (navi_isGPSSignalOK() && (magneticHeading >= 0)) {
164	if (pTargetPos != NULL) { // if there is a target position	164	if (pTargetPos != NULL) { // if there is a target position
165	if (pTargetPos->status != INVALID) { // and the position data are valid	165	if (pTargetPos->status != INVALID) { // and the position data are valid
166	// if the target data are updated or the target pointer has changed	166	// if the target data are updated or the target pointer has changed
167	if ((pTargetPos->status != PROCESSED) \|\| (pTargetPos != pLastTargetPos)) {	167	if ((pTargetPos->status != PROCESSED) \|\| (pTargetPos != pLastTargetPos)) {
168	// reset error integral	168	// reset error integral
169	GPSPosDevIntegral_North = 0;	169	GPSPosDevIntegral_North = 0;
170	GPSPosDevIntegral_East = 0;	170	GPSPosDevIntegral_East = 0;
171	// recalculate latitude projection	171	// recalculate latitude projection
172	cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L);	172	cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L);
173	// remember last target pointer	173	// remember last target pointer
174	pLastTargetPos = pTargetPos;	174	pLastTargetPos = pTargetPos;
175	// mark data as processed	175	// mark data as processed
176	pTargetPos->status = PROCESSED;	176	pTargetPos->status = PROCESSED;
177	}	177	}
178	// calculate position deviation from latitude and longitude differences	178	// calculate position deviation from latitude and longitude differences
179	GPSPosDev_North = (GPSInfo.latitude - pTargetPos->latitude); // to calculate real cm we would need *111/100 additionally	179	GPSPosDev_North = (GPSInfo.latitude - pTargetPos->latitude); // to calculate real cm we would need *111/100 additionally
180	GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally	180	GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally
181	// calculate latitude projection	181	// calculate latitude projection
182	GPSPosDev_East *= cos_target_latitude;	182	GPSPosDev_East *= cos_target_latitude;
183	GPSPosDev_East >>= LOG_MATH_UNIT_FACTOR;	183	GPSPosDev_East >>= LOG_MATH_UNIT_FACTOR;
184	} else { // no valid target position available	184	} else { // no valid target position available
185	// reset error	185	// reset error
186	GPSPosDev_North = 0;	186	GPSPosDev_North = 0;
187	GPSPosDev_East = 0;	187	GPSPosDev_East = 0;
188	// reset error integral	188	// reset error integral
189	GPSPosDevIntegral_North = 0;	189	GPSPosDevIntegral_North = 0;
190	GPSPosDevIntegral_East = 0;	190	GPSPosDevIntegral_East = 0;
191	}	191	}
192	} else { // no target position available	192	} else { // no target position available
193	// reset error	193	// reset error
194	GPSPosDev_North = 0;	194	GPSPosDev_North = 0;
195	GPSPosDev_East = 0;	195	GPSPosDev_East = 0;
196	// reset error integral	196	// reset error integral
197	GPSPosDevIntegral_North = 0;	197	GPSPosDevIntegral_North = 0;
198	GPSPosDevIntegral_East = 0;	198	GPSPosDevIntegral_East = 0;
199	}	199	}
200		200
201	//Calculate PID-components of the controller	201	//Calculate PID-components of the controller
202	// D-Part	202	// D-Part
203	D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9;	203	D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9;
204	D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;	204	D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;
205		205
206	// P-Part	206	// P-Part
207	P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) >> 11;	207	P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) >> 11;
208	P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) >> 11;	208	P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) >> 11;
209		209
210	// I-Part	210	// I-Part
211	I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North) >> 13;	211	I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North) >> 13;
212	I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13;	212	I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13;
213		213
214	// combine P & I	214	// combine P & I
215	PID_North = P_North + I_North;	215	PID_North = P_North + I_North;
216	PID_East = P_East + I_East;	216	PID_East = P_East + I_East;
217		217
218	if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {	218	if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
219	// within limit	219	// within limit
220	GPSPosDevIntegral_North += GPSPosDev_North >> 4;	220	GPSPosDevIntegral_North += GPSPosDev_North >> 4;
221	GPSPosDevIntegral_East += GPSPosDev_East >> 4;	221	GPSPosDevIntegral_East += GPSPosDev_East >> 4;
222	navi_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT);	222	navi_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT);
223	}	223	}
224		224
225	// combine PI- and D-Part	225	// combine PI- and D-Part
226	PID_North += D_North;	226	PID_North += D_North;
227	PID_East += D_East;	227	PID_East += D_East;
228		228
229	// scale combination with gain.	229	// scale combination with gain.
230	// dongfang: Lets not do that. P I and D can be scaled instead.	230	// dongfang: Lets not do that. P I and D can be scaled instead.
231	// PID_North = (PID_North * (int32_t) staticParams.NaviGpsGain) / 100;	231	// PID_North = (PID_North * (int32_t) staticParams.NaviGpsGain) / 100;
232	// PID_East = (PID_East * (int32_t) staticParams.NaviGpsGain) / 100;	232	// PID_East = (PID_East * (int32_t) staticParams.NaviGpsGain) / 100;
233		233
234	// GPS to nick and roll settings	234	// GPS to nick and roll settings
235	// A positive nick angle moves head downwards (flying forward).	235	// A positive nick angle moves head downwards (flying forward).
236	// A positive roll angle tilts left side downwards (flying left).	236	// A positive roll angle tilts left side downwards (flying left).
237	// If compass heading is 0 the head of the copter is in north direction.	237	// If compass heading is 0 the head of the copter is in north direction.
238	// A positive nick angle will fly to north and a positive roll angle will fly to west.	238	// A positive nick angle will fly to north and a positive roll angle will fly to west.
239	// In case of a positive north deviation/velocity the	239	// In case of a positive north deviation/velocity the
240	// copter should fly to south (negative nick).	240	// copter should fly to south (negative nick).
241	// In case of a positive east position deviation and a positive east velocity the	241	// In case of a positive east position deviation and a positive east velocity the
242	// copter should fly to west (positive roll).	242	// copter should fly to west (positive roll).
243	// The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values	243	// The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values
244	// in the flight.c. Therefore a positive north deviation/velocity should result in a positive	244	// in the flight.c. Therefore a positive north deviation/velocity should result in a positive
245	// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.	245	// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
246		246
247	coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW);	247	coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW);
248	sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);	248	sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);
249		249
250	PID_Pitch = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);	250	PID_Pitch = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
251	PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);	251	PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
252		252
253	// limit resulting GPS control vector	253	// limit resulting GPS control vector
254	navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);	254	navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
255		255
256	naviSticks[CONTROL_PITCH] = PID_Pitch;	256	naviSticks[CONTROL_PITCH] = PID_Pitch;
257	naviSticks[CONTROL_ROLL] = PID_Roll;	257	naviSticks[CONTROL_ROLL] = PID_Roll;
258	} else { // invalid GPS data or bad compass reading	258	} else { // invalid GPS data or bad compass reading
259	// reset error integral	259	// reset error integral
260	navi_setNeutral();	260	navi_setNeutral();
261	GPSPosDevIntegral_North = 0;	261	GPSPosDevIntegral_North = 0;
262	GPSPosDevIntegral_East = 0;	262	GPSPosDevIntegral_East = 0;
263	}	263	}
264	}	264	}
265		265
266	void navigation_periodicTaskAndPRTY(int16_t* PRTY) {	266	void navigation_periodicTaskAndPRTY(int16_t* PRTY) {
267	static uint8_t GPS_P_Delay = 0;	267	static uint8_t GPS_P_Delay = 0;
268	static uint16_t beep_rythm = 0;	268	static uint16_t beep_rythm = 0;
269		269
270	static uint16_t navi_testOscPrescaler = 0;	270	static uint16_t navi_testOscPrescaler = 0;
271	static uint8_t navi_testOscTimer = 0;	271	static uint8_t navi_testOscTimer = 0;
272		272
273	navi_testOscPrescaler++;	273	navi_testOscPrescaler++;
274	if (navi_testOscPrescaler == 488) {	274	if (navi_testOscPrescaler == 488) {
275	navi_testOscPrescaler = 0;	275	navi_testOscPrescaler = 0;
276	navi_testOscTimer++;	276	navi_testOscTimer++;
277	if (navi_testOscTimer == staticParams.naviTestOscPeriod) {	277	if (navi_testOscTimer == staticParams.naviTestOscPeriod) {
278	navi_testOscTimer = 0;	278	navi_testOscTimer = 0;
279	if (staticParams.naviTestOscAmplitude) {	279	if (staticParams.naviTestOscAmplitude) {
280	holdPosition->status = NEWDATA;	280	holdPosition.status = NEWDATA;
281	holdPosition->latitude += staticParams.naviTestOscAmplitude * 90L;	281	holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L;
282	}	282	}
283	} else if (navitestOscTimer == staticParams.naviTestOscPeriod/2) {	283	} else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) {
284	if (staticParams.naviTestOscAmplitude) {	284	if (staticParams.naviTestOscAmplitude) {
285	holdPosition->status = NEWDATA;	285	holdPosition.status = NEWDATA;
286	holdPosition->latitude -= staticParams.naviTestOscAmplitude * 90L;	286	holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L;
287	}	287	}
288	}	288	}
289	}	289	}
290		290
291	navi_updateFlightMode();	291	navi_updateFlightMode();
292		292
293	// store home position if start of flight flag is set	293	// store home position if start of flight flag is set
294	if (MKFlags & MKFLAG_CALIBRATE) {	294	if (MKFlags & MKFLAG_CALIBRATE) {
295	MKFlags &= ~(MKFLAG_CALIBRATE);	295	MKFlags &= ~(MKFLAG_CALIBRATE);
296	if (navi_writeCurrPositionTo(&homePosition)) {	296	if (navi_writeCurrPositionTo(&homePosition)) {
297	// shift north to simulate an offset.	297	// shift north to simulate an offset.
298	// homePosition.latitude += 10000L;	298	// homePosition.latitude += 10000L;
299	beep(500);	299	beep(500);
300	}	300	}
301	}	301	}
302		302
303	switch (GPSInfo.status) {	303	switch (GPSInfo.status) {
304	case INVALID: // invalid gps data	304	case INVALID: // invalid gps data
305	navi_setNeutral();	305	navi_setNeutral();
306	naviStatus = NAVI_STATUS_INVALID_GPS;	306	naviStatus = NAVI_STATUS_INVALID_GPS;
307	if (flightMode != NAVI_FLIGHT_MODE_FREE) {	307	if (flightMode != NAVI_FLIGHT_MODE_FREE) {
308	beep(1); // beep if signal is neccesary	308	beep(1); // beep if signal is neccesary
309	}	309	}
310	break;	310	break;
311	case PROCESSED: // if gps data are already processed do nothing	311	case PROCESSED: // if gps data are already processed do nothing
312	// downcount timeout	312	// downcount timeout
313	if (GPSTimeout)	313	if (GPSTimeout)
314	GPSTimeout--;	314	GPSTimeout--;
315	// if no new data arrived within timeout set current data invalid	315	// if no new data arrived within timeout set current data invalid
316	// and therefore disable GPS	316	// and therefore disable GPS
317	else {	317	else {
318	navi_setNeutral();	318	navi_setNeutral();
319	GPSInfo.status = INVALID;	319	GPSInfo.status = INVALID;
320	naviStatus = NAVI_STATUS_GPS_TIMEOUT;	320	naviStatus = NAVI_STATUS_GPS_TIMEOUT;
321	}	321	}
322	break;	322	break;
323	case NEWDATA: // new valid data from gps device	323	case NEWDATA: // new valid data from gps device
324	// if the gps data quality is good	324	// if the gps data quality is good
325	beep_rythm++;	325	beep_rythm++;
326	if (navi_isGPSSignalOK()) {	326	if (navi_isGPSSignalOK()) {
327	switch (flightMode) { // check what's to do	327	switch (flightMode) { // check what's to do
328	case NAVI_FLIGHT_MODE_FREE:	328	case NAVI_FLIGHT_MODE_FREE:
329	// update hold position to current gps position	329	// update hold position to current gps position
330	navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad	330	navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
331	// disable gps control	331	// disable gps control
332	navi_setNeutral();	332	navi_setNeutral();
333	naviStatus = NAVI_STATUS_FREEFLIGHT;	333	naviStatus = NAVI_STATUS_FREEFLIGHT;
334	break;	334	break;
335		335
336	case NAVI_FLIGHT_MODE_AID:	336	case NAVI_FLIGHT_MODE_AID:
337	if (holdPosition.status != INVALID) {	337	if (holdPosition.status != INVALID) {
338	if (navi_isManuallyControlled(PRTY)) { // MK controlled by user	338	if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
339	// update hold point to current gps position	339	// update hold point to current gps position
340	navi_writeCurrPositionTo(&holdPosition);	340	navi_writeCurrPositionTo(&holdPosition);
341	// disable gps control	341	// disable gps control
342	navi_setNeutral();	342	navi_setNeutral();
343	GPS_P_Delay = 0;	343	GPS_P_Delay = 0;
344	naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;	344	naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
345	} else { // GPS control active	345	} else { // GPS control active
346	if (GPS_P_Delay < 7) {	346	if (GPS_P_Delay < 7) {
347	// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)	347	// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
348	GPS_P_Delay++;	348	GPS_P_Delay++;
349	navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position	349	navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
350	navi_PIDController(NULL); // activates only the D-Part	350	navi_PIDController(NULL); // activates only the D-Part
351	naviStatus = NAVI_STATUS_POSITION_HOLD;	351	naviStatus = NAVI_STATUS_POSITION_HOLD;
352	} else {	352	} else {
353	navi_PIDController(&holdPosition); // activates the P&D-Part	353	navi_PIDController(&holdPosition); // activates the P&D-Part
354	naviStatus = NAVI_STATUS_POSITION_HOLD;	354	naviStatus = NAVI_STATUS_POSITION_HOLD;
355	}	355	}
356	}	356	}
357	} else { // invalid Hold Position	357	} else { // invalid Hold Position
358	// try to catch a valid hold position from gps data input	358	// try to catch a valid hold position from gps data input
359	navi_writeCurrPositionTo(&holdPosition);	359	navi_writeCurrPositionTo(&holdPosition);
360	navi_setNeutral();	360	navi_setNeutral();
361	naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID;	361	naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID;
362	}	362	}
363	break;	363	break;
364		364
365	case NAVI_FLIGHT_MODE_HOME:	365	case NAVI_FLIGHT_MODE_HOME:
366	if (homePosition.status != INVALID) {	366	if (homePosition.status != INVALID) {
367	// update hold point to current gps position	367	// update hold point to current gps position
368	// to avoid a flight back if home comming is deactivated	368	// to avoid a flight back if home comming is deactivated
369	navi_writeCurrPositionTo(&holdPosition);	369	navi_writeCurrPositionTo(&holdPosition);
370	if (navi_isManuallyControlled(PRTY)) { // MK controlled by user	370	if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
371	navi_setNeutral();	371	navi_setNeutral();
372	naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;	372	naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
373	} else {// GPS control active	373	} else {// GPS control active
374	navi_PIDController(&homePosition);	374	navi_PIDController(&homePosition);
375	}	375	}
376	naviStatus = NAVI_STATUS_RTH;	376	naviStatus = NAVI_STATUS_RTH;
377	} else {	377	} else {
378	// bad home position	378	// bad home position
379	beep(50); // signal invalid home position	379	beep(50); // signal invalid home position
380	// try to hold at least the position as a fallback option	380	// try to hold at least the position as a fallback option
381	if (holdPosition.status != INVALID) {	381	if (holdPosition.status != INVALID) {
382	if (navi_isManuallyControlled(PRTY)) {	382	if (navi_isManuallyControlled(PRTY)) {
383	// MK controlled by user	383	// MK controlled by user
384	navi_setNeutral();	384	navi_setNeutral();
385	naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;	385	naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
386	} else {	386	} else {
387	// GPS control active	387	// GPS control active
388	navi_PIDController(&holdPosition);	388	navi_PIDController(&holdPosition);
389	naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD;	389	naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD;
390	}	390	}
391	} else { // try to catch a valid hold position	391	} else { // try to catch a valid hold position
392	navi_writeCurrPositionTo(&holdPosition);	392	navi_writeCurrPositionTo(&holdPosition);
393	naviStatus = NAVI_STATUS_RTH_POSITION_INVALID;	393	naviStatus = NAVI_STATUS_RTH_POSITION_INVALID;
394	navi_setNeutral();	394	navi_setNeutral();
395	}	395	}
396	}	396	}
397	break; // eof TSK_HOME	397	break; // eof TSK_HOME
398	default: // unhandled task	398	default: // unhandled task
399	navi_setNeutral();	399	navi_setNeutral();
400	break; // eof default	400	break; // eof default
401	} // eof switch GPS_Task	401	} // eof switch GPS_Task
402	} // eof gps data quality is good	402	} // eof gps data quality is good
403	else { // gps data quality is bad	403	else { // gps data quality is bad
404	// disable gps control	404	// disable gps control
405	navi_setNeutral();	405	navi_setNeutral();
406	naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;	406	naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;
407	if (flightMode != NAVI_FLIGHT_MODE_FREE) {	407	if (flightMode != NAVI_FLIGHT_MODE_FREE) {
408	// beep if signal is not sufficient	408	// beep if signal is not sufficient
409	if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))	409	if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
410	beep(100);	410	beep(100);
411	else if (GPSInfo.satnum < staticParams.GPSMininumSatellites	411	else if (GPSInfo.satnum < staticParams.GPSMininumSatellites
412	&& !(beep_rythm % 5))	412	&& !(beep_rythm % 5))
413	beep(10);	413	beep(10);
414	}	414	}
415	}	415	}
416	// set current data as processed to avoid further calculations on the same gps data	416	// set current data as processed to avoid further calculations on the same gps data
417	GPSInfo.status = PROCESSED;	417	GPSInfo.status = PROCESSED;
418	break;	418	break;
419	} // eof GPSInfo.status	419	} // eof GPSInfo.status
420		420
421	PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];	421	PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
422	PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];	422	PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
423		423
424	debugOut.analog[16] = flightMode;	424	debugOut.analog[16] = flightMode;
425	debugOut.analog[17] = naviStatus;	425	debugOut.analog[17] = naviStatus;
426		426
427	debugOut.analog[18] = naviSticks[CONTROL_PITCH];	427	debugOut.analog[18] = naviSticks[CONTROL_PITCH];
428	debugOut.analog[19] = naviSticks[CONTROL_ROLL];	428	debugOut.analog[19] = naviSticks[CONTROL_ROLL];
429	}	429	}
430		430

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c @ 2090 - Rev 2086 → 2087