Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2071 → Rev 2074

/branches/dongfang_FC_rewrite/directGPSNaviControl.c
13,12 → 13,25
#include "attitude.h"
 
typedef enum {
GPS_FLIGHT_MODE_UNDEF,
GPS_FLIGHT_MODE_FREE,
GPS_FLIGHT_MODE_AID,
GPS_FLIGHT_MODE_HOME,
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
35,22 → 48,24
// GPS coordinates for home position
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
// the current flight mode
FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
int16_t naviSticks[2] = {0,0};
 
uint8_t naviStatus;
 
// ---------------------------------------------------------------------------------
void navi_updateFlightMode(void) {
static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
 
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
flightMode = GPS_FLIGHT_MODE_FREE;
flightMode = NAVI_FLIGHT_MODE_FREE;
} else {
if (dynamicParams.naviMode < 50)
flightMode = GPS_FLIGHT_MODE_FREE;
flightMode = NAVI_FLIGHT_MODE_FREE;
else if (dynamicParams.naviMode < 180)
flightMode = GPS_FLIGHT_MODE_AID;
flightMode = NAVI_FLIGHT_MODE_AID;
else
flightMode = GPS_FLIGHT_MODE_HOME;
flightMode = NAVI_FLIGHT_MODE_HOME;
}
 
if (flightMode != flightModeOld) {
260,7 → 275,7
if (MKFlags & MKFLAG_CALIBRATE) {
MKFlags &= ~(MKFLAG_CALIBRATE);
if (navi_writeCurrPositionTo(&homePosition)) {
// shift north to simulate an offset.
// shift north to simulate an offset.
// homePosition.latitude += 10000L;
beep(500);
}
269,7 → 284,8
switch (GPSInfo.status) {
case INVALID: // invalid gps data
navi_setNeutral();
if (flightMode != GPS_FLIGHT_MODE_FREE) {
naviStatus = NAVI_STATUS_INVALID_GPS;
if (flightMode != NAVI_FLIGHT_MODE_FREE) {
beep(1); // beep if signal is neccesary
}
break;
282,6 → 298,7
else {
navi_setNeutral();
GPSInfo.status = INVALID;
naviStatus = NAVI_STATUS_GPS_TIMEOUT;
}
break;
case NEWDATA: // new valid data from gps device
289,14 → 306,15
beep_rythm++;
if (navi_isGPSSignalOK()) {
switch (flightMode) { // check what's to do
case GPS_FLIGHT_MODE_FREE:
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 GPS_FLIGHT_MODE_AID:
case NAVI_FLIGHT_MODE_AID:
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
// update hold point to current gps position
304,6 → 322,7
// 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)
310,17 → 329,21
GPS_P_Delay++;
navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
navi_PIDController(NULL); // activates only the D-Part
} else
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 GPS_FLIGHT_MODE_HOME:
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
330,21 → 353,24
} 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();
}
}
357,7 → 383,8
else { // gps data quality is bad
// disable gps control
navi_setNeutral();
if (flightMode != GPS_FLIGHT_MODE_FREE) {
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);
373,4 → 400,10
 
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];
}