WebSVN - FlightCtrl - Diff - Rev 2047 and 2048 - /branches/dongfang_FC



#include "controlMixer.h"
#include "output.h"
#include "isqrt.h"
#include "attitude.h"
#include "dongfangMath.h"
#include "attitude.h"

 
typedef enum {
  GPS_FLIGHT_MODE_UNDEF,
  GPS_FLIGHT_MODE_FREE,

  if (controlMixer_getSignalQuality() <= SIGNAL_BAD
      || MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
    flightMode = GPS_FLIGHT_MODE_HOME;
  } else {
    if (dynamicParams.naviMode < 50)
      flightMode = GPS_FLIGHT_MODE_HOME;
    else if (dynamicParams.naviMode < 180)
      flightMode = GPS_FLIGHT_MODE_AID;
    else
      flightMode = GPS_FLIGHT_MODE_FREE;
  }

 
  if (flightMode != flightModeOld) {
    beep(100);

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

}

 
// 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, int16_t *PRTY) {
  static int32_t PID_Nick, PID_Roll;
  int32_t coscompass, sincompass;
  int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
  int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;

    // 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_Nick = (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_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
 
    PRTY[CONTROL_PITCH] += PID_Nick;
    PRTY[CONTROL_ROLL]  += PID_Roll;
 
  } else { // invalid GPS data or bad compass reading

    // reset error integral
    GPSPosDevIntegral_North = 0;
    GPSPosDevIntegral_East = 0;

  }

        // disable gps control
        break;

 
      case GPS_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
            GPS_P_Delay = 0;
          } else { // GPS control active
            if (GPS_P_Delay < 7) {
              // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
              GPS_P_Delay++;
              navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
              navi_PIDController(NULL, PRTY); // activates only the D-Part
            } else
              navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part
          }
        } else // invalid Hold Position
        { // try to catch a valid hold position from gps data input
          navi_writeCurrPositionTo(&holdPosition);

      case GPS_FLIGHT_MODE_HOME:
        if (homePosition.status != INVALID) {
          // update hold point to current gps position
          // to avoid a flight back if home comming is deactivated
          navi_writeCurrPositionTo(&holdPosition);
          if (navi_isManuallyControlled(PRTY)) // MK controlled by user
          {
          } else {// GPS control active
            navi_PIDController(&homePosition, PRTY);
          }
        } 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
            } else {
              // GPS control active
              navi_PIDController(&holdPosition, PRTY);
            }
          } else { // try to catch a valid hold position
            navi_writeCurrPositionTo(&holdPosition);
          }

    break;
  } // eof GPSInfo.status

 
  debugOut.analog[11] = GPSInfo.satnum;
 
 

Rev 2047	Rev 2048
Line 13...	Line 13...
13	#include "controlMixer.h"	13	#include "controlMixer.h"
14	#include "output.h"	14	#include "output.h"
15	#include "isqrt.h"	15	#include "isqrt.h"
16	#include "attitude.h"	16	#include "attitude.h"
17	#include "dongfangMath.h"	17	#include "dongfangMath.h"
-		18	#include "attitude.h"
Line 18...	Line 19...
18		19
19	typedef enum {	20	typedef enum {
20	GPS_FLIGHT_MODE_UNDEF,	21	GPS_FLIGHT_MODE_UNDEF,
21	GPS_FLIGHT_MODE_FREE,	22	GPS_FLIGHT_MODE_FREE,
Line 48...	Line 49...
48	if (controlMixer_getSignalQuality() <= SIGNAL_BAD	49	if (controlMixer_getSignalQuality() <= SIGNAL_BAD
49	\|\| MKFlags & MKFLAG_EMERGENCY_FLIGHT) {	50	\|\| MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
50	flightMode = GPS_FLIGHT_MODE_HOME;	51	flightMode = GPS_FLIGHT_MODE_HOME;
51	} else {	52	} else {
52	if (dynamicParams.naviMode < 50)	53	if (dynamicParams.naviMode < 50)
53	flightMode = GPS_FLIGHT_MODE_FREE;	54	flightMode = GPS_FLIGHT_MODE_HOME;
54	else if (dynamicParams.naviMode < 180)	55	else if (dynamicParams.naviMode < 180)
55	flightMode = GPS_FLIGHT_MODE_AID;	56	flightMode = GPS_FLIGHT_MODE_AID;
56	else	57	else
57	flightMode = GPS_FLIGHT_MODE_HOME;	58	flightMode = GPS_FLIGHT_MODE_FREE;
58	}	59	}
Line 59...	Line 60...
59		60
60	if (flightMode != flightModeOld) {	61	if (flightMode != flightModeOld) {
61	beep(100);	62	beep(100);
Line 91...	Line 92...
91	}	92	}
92	return 0;	93	return 0;
93	}	94	}
Line 94...	Line 95...
94		95
95	// checks nick and roll sticks for manual control	96	// checks nick and roll sticks for manual control
96	uint8_t navi_isManuallyControlled(int16_t* sticks) {	97	uint8_t navi_isManuallyControlled(int16_t* PRTY) {
97	if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold	98	if (PRTY[CONTROL_PITCH] < staticParams.naviStickThreshold
98	&& sticks[CONTROL_ROLL] < staticParams.naviStickThreshold)	99	&& PRTY[CONTROL_ROLL] < staticParams.naviStickThreshold)
99	return 0;	100	return 0;
100	else	101	else
101	return 1;	102	return 1;
Line 132...	Line 133...
132	}	133	}
Line 133...	Line 134...
133		134
134	// calculates the GPS control stick values from the deviation to target position	135	// calculates the GPS control stick values from the deviation to target position
135	// if the pointer to the target positin is NULL or is the target position invalid	136	// if the pointer to the target positin is NULL or is the target position invalid
136	// then the P part of the controller is deactivated.	137	// then the P part of the controller is deactivated.
137	void navi_PIDController(GPS_Pos_t pTargetPos, int16_t sticks) {	138	void navi_PIDController(GPS_Pos_t pTargetPos, int16_t PRTY) {
138	static int32_t PID_Nick, PID_Roll;	139	static int32_t PID_Nick, PID_Roll;
139	int32_t coscompass, sincompass;	140	int32_t coscompass, sincompass;
140	int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm	141	int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
141	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;	142	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
Line 228...	Line 229...
228	// copter should fly to west (positive roll).	229	// copter should fly to west (positive roll).
229	// The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values	230	// The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values
230	// in the flight.c. Therefore a positive north deviation/velocity should result in a positive	231	// in the flight.c. Therefore a positive north deviation/velocity should result in a positive
231	// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.	232	// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
Line 232...	Line 233...
232		233
233	coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);	234	coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW);
Line 234...	Line 235...
234	sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);	235	sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);
235		236
Line 236...	Line 237...
236	PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);	237	PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
237	PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);	238	PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
Line 238...	Line -...
238		-
239	// limit resulting GPS control vector	-
240	navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);	-
241		239
242	debugOut.analog[27] = -PID_Nick;	240	// limit resulting GPS control vector
Line 243...	Line 241...
243	debugOut.analog[28] = -PID_Roll;	241	navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
244		242
245	sticks[CONTROL_PITCH] -= PID_Nick;	243	PRTY[CONTROL_PITCH] += PID_Nick;
246	sticks[CONTROL_ROLL] -= PID_Roll;	244	PRTY[CONTROL_ROLL] += PID_Roll;
247		245
248	} else { // invalid GPS data or bad compass reading	246	} else { // invalid GPS data or bad compass reading
Line 249...	Line 247...
249	// reset error integral	247	// reset error integral
250	GPSPosDevIntegral_North = 0;	248	GPSPosDevIntegral_North = 0;
251	GPSPosDevIntegral_East = 0;	249	GPSPosDevIntegral_East = 0;
Line 252...	Line 250...
252	}	250	}
Line 294...	Line 292...
294	// disable gps control	292	// disable gps control
295	break;	293	break;
Line 296...	Line 294...
296		294
297	case GPS_FLIGHT_MODE_AID:	295	case GPS_FLIGHT_MODE_AID:
298	if (holdPosition.status != INVALID) {	296	if (holdPosition.status != INVALID) {
299	if (navi_isManuallyControlled(sticks)) { // MK controlled by user	297	if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
300	// update hold point to current gps position	298	// update hold point to current gps position
301	navi_writeCurrPositionTo(&holdPosition);	299	navi_writeCurrPositionTo(&holdPosition);
302	// disable gps control	300	// disable gps control
303	GPS_P_Delay = 0;	301	GPS_P_Delay = 0;
304	} else { // GPS control active	302	} else { // GPS control active
305	if (GPS_P_Delay < 7) {	303	if (GPS_P_Delay < 7) {
306	// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)	304	// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
307	GPS_P_Delay++;	305	GPS_P_Delay++;
308	navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position	306	navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
309	navi_PIDController(NULL, sticks); // activates only the D-Part	307	navi_PIDController(NULL, PRTY); // activates only the D-Part
310	} else	308	} else
311	navi_PIDController(&holdPosition, sticks); // activates the P&D-Part	309	navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part
312	}	310	}
313	} else // invalid Hold Position	311	} else // invalid Hold Position
314	{ // try to catch a valid hold position from gps data input	312	{ // try to catch a valid hold position from gps data input
315	navi_writeCurrPositionTo(&holdPosition);	313	navi_writeCurrPositionTo(&holdPosition);
Line 319...	Line 317...
319	case GPS_FLIGHT_MODE_HOME:	317	case GPS_FLIGHT_MODE_HOME:
320	if (homePosition.status != INVALID) {	318	if (homePosition.status != INVALID) {
321	// update hold point to current gps position	319	// update hold point to current gps position
322	// to avoid a flight back if home comming is deactivated	320	// to avoid a flight back if home comming is deactivated
323	navi_writeCurrPositionTo(&holdPosition);	321	navi_writeCurrPositionTo(&holdPosition);
324	if (navi_isManuallyControlled(sticks)) // MK controlled by user	322	if (navi_isManuallyControlled(PRTY)) // MK controlled by user
325	{	323	{
326	} else {// GPS control active	324	} else {// GPS control active
327	navi_PIDController(&homePosition, sticks);	325	navi_PIDController(&homePosition, PRTY);
328	}	326	}
329	} else {	327	} else {
330	// bad home position	328	// bad home position
331	beep(50); // signal invalid home position	329	beep(50); // signal invalid home position
332	// try to hold at least the position as a fallback option	330	// try to hold at least the position as a fallback option
Line 333...	Line 331...
333		331
334	if (holdPosition.status != INVALID) {	332	if (holdPosition.status != INVALID) {
335	if (navi_isManuallyControlled(sticks)) {	333	if (navi_isManuallyControlled(PRTY)) {
336	// MK controlled by user	334	// MK controlled by user
337	} else {	335	} else {
338	// GPS control active	336	// GPS control active
339	navi_PIDController(&holdPosition, sticks);	337	navi_PIDController(&holdPosition, PRTY);
340	}	338	}
341	} else { // try to catch a valid hold position	339	} else { // try to catch a valid hold position
342	navi_writeCurrPositionTo(&holdPosition);	340	navi_writeCurrPositionTo(&holdPosition);
343	}	341	}
Line 363...	Line 361...
363	break;	361	break;
364	} // eof GPSInfo.status	362	} // eof GPSInfo.status
Line 365...	Line 363...
365		363
366	debugOut.analog[11] = GPSInfo.satnum;	364	debugOut.analog[11] = GPSInfo.satnum;
367	}	-
368		-

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c - Rev 2047 → 2048