Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2045 → Rev 2046

/branches/dongfang_FC_rewrite/directGPSNaviControl.c
24,8 → 24,8
} FlightMode_t;
 
#define GPS_POSINTEGRAL_LIMIT 32000
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
#define GPS_P_LIMIT 25
#define GPS_STICK_LIMIT 400 // limit of gps stick control to avoid critical flight attitudes
#define GPS_P_LIMIT 100
 
typedef struct {
int32_t longitude;
42,7 → 42,7
FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;
 
// ---------------------------------------------------------------------------------
void GPS_updateFlightMode(void) {
void navi_updateFlightMode(void) {
static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD
67,7 → 67,7
 
// ---------------------------------------------------------------------------------
// This function defines a good GPS signal condition
uint8_t GPS_isSignalOK(void) {
uint8_t navi_isGPSSignalOK(void) {
static uint8_t GPSFix = 0;
if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)
&& (GPSInfo.flags & FLAG_GPSFIXOK)
80,7 → 80,7
 
// ---------------------------------------------------------------------------------
// rescale xy-vector length to limit
uint8_t GPS_limitXY(int32_t *x, int32_t *y, int32_t 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) {
93,7 → 93,7
}
 
// checks nick and roll sticks for manual control
uint8_t GPS_isManuallyControlled(int16_t* sticks) {
uint8_t navi_isManuallyControlled(int16_t* sticks) {
if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold
&& sticks[CONTROL_ROLL] < staticParams.naviStickThreshold)
return 0;
102,11 → 102,11
}
 
// set given position to current gps position
uint8_t GPS_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
uint8_t navi_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
if (pGPSPos == NULL)
return 0; // bad pointer
 
if (GPS_isSignalOK()) { // is GPS signal condition is fine
if (navi_isGPSSignalOK()) { // is GPS signal condition is fine
pGPSPos->longitude = GPSInfo.longitude;
pGPSPos->latitude = GPSInfo.latitude;
pGPSPos->altitude = GPSInfo.altitude;
119,7 → 119,7
}
 
// clear position
uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) {
uint8_t navi_clearPosition(GPS_Pos_t * pGPSPos) {
if (pGPSPos == NULL)
return 0; // bad pointer
else {
134,7 → 134,7
// 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 GPS_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) {
void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) {
static int32_t PID_Nick, PID_Roll;
int32_t coscompass, sincompass;
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
145,7 → 145,7
static GPS_Pos_t *pLastTargetPos = 0;
 
// if GPS data and Compass are ok
if (GPS_isSignalOK() && (magneticHeading >= 0)) {
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
187,26 → 187,25
//Calculate PID-components of the controller
 
// D-Part
D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) / 512;
D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) / 512;
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) / 2048;
P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) / 2048;
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)
/ 8192;
I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) / 8192;
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 (!GPS_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
GPSPosDevIntegral_North += GPSPosDev_North / 16;
GPSPosDevIntegral_East += GPSPosDev_East / 16;
GPS_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East,
GPS_POSINTEGRAL_LIMIT);
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
234,17 → 233,17
coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
 
PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> MATH_UNIT_FACTOR_LOG;
PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG;
PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2);
PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2);
 
// limit resulting GPS control vector
GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
navi_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
 
debugOut.analog[27] = PID_Nick;
debugOut.analog[28] = PID_Roll;
debugOut.analog[27] = -PID_Nick;
debugOut.analog[28] = -PID_Roll;
 
sticks[CONTROL_PITCH] += PID_Nick;
sticks[CONTROL_ROLL] += PID_Roll;
sticks[CONTROL_PITCH] -= PID_Nick;
sticks[CONTROL_ROLL] -= PID_Roll;
 
} else { // invalid GPS data or bad compass reading
// reset error integral
257,13 → 256,13
static uint8_t GPS_P_Delay = 0;
static uint16_t beep_rythm = 0;
 
GPS_updateFlightMode();
navi_updateFlightMode();
 
// store home position if start of flight flag is set
if (MKFlags & MKFLAG_CALIBRATE) {
MKFlags &= ~(MKFLAG_CALIBRATE);
if (GPS_writeCurrPositionTo(&homePosition)) {
homePosition.latitude += 9000L;
if (navi_writeCurrPositionTo(&homePosition)) {
homePosition.latitude += 10000L;
beep(500);
}
}
287,19 → 286,19
case NEWDATA: // new valid data from gps device
// if the gps data quality is good
beep_rythm++;
if (GPS_isSignalOK()) {
if (navi_isGPSSignalOK()) {
switch (flightMode) { // check what's to do
case GPS_FLIGHT_MODE_FREE:
// update hold position to current gps position
GPS_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
// disable gps control
break;
 
case GPS_FLIGHT_MODE_AID:
if (holdPosition.status != INVALID) {
if (GPS_isManuallyControlled(sticks)) { // MK controlled by user
if (navi_isManuallyControlled(sticks)) { // MK controlled by user
// update hold point to current gps position
GPS_writeCurrPositionTo(&holdPosition);
navi_writeCurrPositionTo(&holdPosition);
// disable gps control
GPS_P_Delay = 0;
} else { // GPS control active
306,14 → 305,14
if (GPS_P_Delay < 7) {
// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay++;
GPS_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
GPS_PIDController(NULL, sticks); // activates only the D-Part
navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
navi_PIDController(NULL, sticks); // activates only the D-Part
} else
GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part
navi_PIDController(&holdPosition, sticks); // activates the P&D-Part
}
} else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
GPS_writeCurrPositionTo(&holdPosition);
navi_writeCurrPositionTo(&holdPosition);
}
break;
 
321,11 → 320,11
if (homePosition.status != INVALID) {
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
GPS_writeCurrPositionTo(&holdPosition);
if (GPS_isManuallyControlled(sticks)) // MK controlled by user
navi_writeCurrPositionTo(&holdPosition);
if (navi_isManuallyControlled(sticks)) // MK controlled by user
{
} else {// GPS control active
GPS_PIDController(&homePosition, sticks);
navi_PIDController(&homePosition, sticks);
}
} else {
// bad home position
333,14 → 332,14
// try to hold at least the position as a fallback option
 
if (holdPosition.status != INVALID) {
if (GPS_isManuallyControlled(sticks)) {
if (navi_isManuallyControlled(sticks)) {
// MK controlled by user
} else {
// GPS control active
GPS_PIDController(&holdPosition, sticks);
navi_PIDController(&holdPosition, sticks);
}
} else { // try to catch a valid hold position
GPS_writeCurrPositionTo(&holdPosition);
navi_writeCurrPositionTo(&holdPosition);
}
}
break; // eof TSK_HOME