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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c @ 2097 - Rev 2071 → 2074