WebSVN - FlightCtrl - Diff - Rev 2160 and 2189 - /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 "attitude.h"
 
typedef enum {
  NAVI_FLIGHT_MODE_UNDEF,
  NAVI_FLIGHT_MODE_FREE,
  NAVI_FLIGHT_MODE_AID,
  NAVI_FLIGHT_MODE_HOME,
} NaviMode_t;
 
/*
 * This is not read from internally. It only serves to let a pilot/debugger
 * know the status of the navigation system (thru a data connection).
 */
typedef enum {
  NAVI_STATUS_NO_COMPASS=-1,
  NAVI_STATUS_INVALID_GPS=-2,
  NAVI_STATUS_BAD_GPS_SIGNAL=-3,
  NAVI_STATUS_RTH_POSITION_INVALID=-4,
  NAVI_STATUS_RTH_FALLBACK_ON_HOLD=-5,
  NAVI_STATUS_GPS_TIMEOUT=-6,
  NAVI_STATUS_FREEFLIGHT=0,
  NAVI_STATUS_POSITION_HOLD=1,
  NAVI_STATUS_RTH=2,
  NAVI_STATUS_MANUAL_OVERRIDE=3,
  NAVI_STATUS_HOLD_POSITION_INVALID=4,
} 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
NaviMode_t naviMode = NAVI_FLIGHT_MODE_UNDEF;
int16_t naviSticks[2] = {0,0};
 
int8_t naviStatus;
 
// ---------------------------------------------------------------------------------
void navi_updateFlightMode(void) {
  static NaviMode_t naviModeOld = NAVI_FLIGHT_MODE_UNDEF;
 
  if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
    naviMode = NAVI_FLIGHT_MODE_FREE;
  } else {
    if (dynamicParams.naviMode < 50)
      naviMode = NAVI_FLIGHT_MODE_FREE;
    else if (dynamicParams.naviMode < 180)
      naviMode = NAVI_FLIGHT_MODE_AID;
    else
      naviMode = NAVI_FLIGHT_MODE_HOME;
  }
 
  if (naviMode != naviModeOld) {
    beep(100);
    naviModeOld = naviMode;
  }
}
 
// ---------------------------------------------------------------------------------
// 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* RPTY) {
 
 
  if (abs(RPTY[CONTROL_PITCH]) < staticParams.naviStickThreshold
      && abs(RPTY[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_periodicTaskAndRPTY(int16_t* RPTY) {
  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;
 
  if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) {
    navi_setNeutral();
    naviStatus = NAVI_STATUS_NO_COMPASS;
    return;
  }
 
  navi_updateNaviMode();
 
  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; // there are about 90 units per meter.
          }
      } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) {
          if (staticParams.naviTestOscAmplitude) {
              holdPosition.status = NEWDATA;
              holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L;
          }
      }
  }
 
  // 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 (naviMode != 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 (naviMode) { // 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(RPTY)) { // 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(RPTY)) { // 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(RPTY)) {
              // 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 (naviMode != 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
 
  RPTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
 
 
 
 
 
 
 
 
  RPTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c @ 2289 - Rev 2160 → 2189