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