9,7 → 9,6 |
#include "output.h" |
#include "isqrt.h" |
#include "attitude.h" |
#include "dongfangMath.h" |
#include "attitude.h" |
|
typedef enum { |
17,7 → 16,7 |
NAVI_FLIGHT_MODE_FREE, |
NAVI_FLIGHT_MODE_AID, |
NAVI_FLIGHT_MODE_HOME, |
} FlightMode_t; |
} NaviMode_t; |
|
/* |
* This is not read from internally. It only serves to let a pilot/debugger |
53,7 → 52,7 |
// GPS coordinates for home position |
GPS_Pos_t homePosition = { 0, 0, 0, INVALID }; |
// the current flight mode |
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF; |
NaviMode_t naviMode = NAVI_FLIGHT_MODE_UNDEF; |
int16_t naviSticks[2] = {0,0}; |
|
int8_t naviStatus; |
60,22 → 59,22 |
|
// --------------------------------------------------------------------------------- |
void navi_updateFlightMode(void) { |
static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF; |
static NaviMode_t naviModeOld = NAVI_FLIGHT_MODE_UNDEF; |
|
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
flightMode = NAVI_FLIGHT_MODE_FREE; |
naviMode = NAVI_FLIGHT_MODE_FREE; |
} else { |
if (dynamicParams.naviMode < 50) |
flightMode = NAVI_FLIGHT_MODE_FREE; |
naviMode = NAVI_FLIGHT_MODE_FREE; |
else if (dynamicParams.naviMode < 180) |
flightMode = NAVI_FLIGHT_MODE_AID; |
naviMode = NAVI_FLIGHT_MODE_AID; |
else |
flightMode = NAVI_FLIGHT_MODE_HOME; |
naviMode = NAVI_FLIGHT_MODE_HOME; |
} |
|
if (flightMode != flightModeOld) { |
if (naviMode != naviModeOld) { |
beep(100); |
flightModeOld = flightMode; |
naviModeOld = naviMode; |
} |
} |
|
107,11 → 106,9 |
} |
|
// checks nick and roll sticks for manual control |
uint8_t navi_isManuallyControlled(int16_t* PRTY) { |
debugOut.analog[26] = PRTY[CONTROL_PITCH]; |
debugOut.analog[27] = PRTY[CONTROL_ROLL]; |
if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold |
&& abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold) |
uint8_t navi_isManuallyControlled(int16_t* RPTY) { |
if (abs(RPTY[CONTROL_PITCH]) < staticParams.naviStickThreshold |
&& abs(RPTY[CONTROL_ROLL]) < staticParams.naviStickThreshold) |
return 0; |
else |
return 1; |
268,7 → 265,7 |
} |
} |
|
void navigation_periodicTaskAndPRTY(int16_t* PRTY) { |
void navigation_periodicTaskAndRPTY(int16_t* RPTY) { |
static uint8_t GPS_P_Delay = 0; |
static uint16_t beep_rythm = 0; |
static uint16_t navi_testOscPrescaler = 0; |
280,7 → 277,7 |
return; |
} |
|
navi_updateFlightMode(); |
navi_updateNaviMode(); |
|
navi_testOscPrescaler++; |
if (navi_testOscPrescaler == 488) { |
314,7 → 311,7 |
case INVALID: // invalid gps data |
navi_setNeutral(); |
naviStatus = NAVI_STATUS_INVALID_GPS; |
if (flightMode != NAVI_FLIGHT_MODE_FREE) { |
if (naviMode != NAVI_FLIGHT_MODE_FREE) { |
beep(1); // beep if signal is neccesary |
} |
break; |
334,7 → 331,7 |
// if the gps data quality is good |
beep_rythm++; |
if (navi_isGPSSignalOK()) { |
switch (flightMode) { // check what's to do |
switch (naviMode) { // 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 |
345,7 → 342,7 |
|
case NAVI_FLIGHT_MODE_AID: |
if (holdPosition.status != INVALID) { |
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
if (navi_isManuallyControlled(RPTY)) { // MK controlled by user |
// update hold point to current gps position |
navi_writeCurrPositionTo(&holdPosition); |
// disable gps control |
377,7 → 374,7 |
// 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 |
if (navi_isManuallyControlled(RPTY)) { // MK controlled by user |
navi_setNeutral(); |
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE; |
} else {// GPS control active |
389,7 → 386,7 |
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)) { |
if (navi_isManuallyControlled(RPTY)) { |
// MK controlled by user |
navi_setNeutral(); |
naviStatus = NAVI_STATUS_MANUAL_OVERRIDE; |
414,7 → 411,7 |
// disable gps control |
navi_setNeutral(); |
naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL; |
if (flightMode != NAVI_FLIGHT_MODE_FREE) { |
if (naviMode != NAVI_FLIGHT_MODE_FREE) { |
// beep if signal is not sufficient |
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
beep(100); |
428,14 → 425,6 |
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]; |
*/ |
RPTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH]; |
RPTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL]; |
} |