1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00

Merge pull request #7617 from breadoven/abo_remove_NAV_define

Remove USE_NAV define
This commit is contained in:
Paweł Spychalski 2021-12-21 09:01:39 +01:00 committed by GitHub
commit ac6650e25f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
42 changed files with 26 additions and 389 deletions

View file

@ -653,18 +653,11 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
#endif #endif
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV: case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
#ifdef USE_NAV
return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID); return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV: case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
#ifdef USE_NAV
return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID); return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_RSSI: case FLIGHT_LOG_FIELD_CONDITION_RSSI:
// Assumes blackboxStart() is called after rxInit(), which should be true since // Assumes blackboxStart() is called after rxInit(), which should be true since
@ -1468,9 +1461,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->time = currentTimeUs; blackboxCurrent->time = currentTimeUs;
#ifdef USE_NAV
const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
#endif
for (int i = 0; i < XYZ_AXIS_COUNT; i++) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i]; blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i];
@ -1483,7 +1474,6 @@ static void loadMainState(timeUs_t currentTimeUs)
#ifdef USE_MAG #ifdef USE_MAG
blackboxCurrent->magADC[i] = mag.magADC[i]; blackboxCurrent->magADC[i] = mag.magADC[i];
#endif #endif
#ifdef USE_NAV
if (!STATE(FIXED_WING_LEGACY)) { if (!STATE(FIXED_WING_LEGACY)) {
// log requested velocity in cm/s // log requested velocity in cm/s
blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained); blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained);
@ -1495,10 +1485,8 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward); blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward);
blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained); blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
} }
#endif
} }
#ifdef USE_NAV
if (STATE(FIXED_WING_LEGACY)) { if (STATE(FIXED_WING_LEGACY)) {
// log requested pitch in decidegrees // log requested pitch in decidegrees
@ -1519,7 +1507,6 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10); blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10);
blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10); blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10);
} }
#endif
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
blackboxCurrent->rcData[i] = rxGetChannelValue(i); blackboxCurrent->rcData[i] = rxGetChannelValue(i);

View file

@ -101,9 +101,7 @@ static const OSD_Entry menuFeaturesEntries[] =
OSD_LABEL_ENTRY("--- FEATURES ---"), OSD_LABEL_ENTRY("--- FEATURES ---"),
OSD_SUBMENU_ENTRY("BLACKBOX", &cmsx_menuBlackbox), OSD_SUBMENU_ENTRY("BLACKBOX", &cmsx_menuBlackbox),
OSD_SUBMENU_ENTRY("MIXER & SERVOS", &cmsx_menuMixerServo), OSD_SUBMENU_ENTRY("MIXER & SERVOS", &cmsx_menuMixerServo),
#if defined(USE_NAV)
OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation), OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation),
#endif
#if defined(USE_VTX_CONTROL) #if defined(USE_VTX_CONTROL)
OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl), OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtxControl),
#endif // VTX_CONTROL #endif // VTX_CONTROL

View file

@ -26,8 +26,6 @@
#include "platform.h" #include "platform.h"
#if defined(USE_NAV)
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
@ -234,5 +232,3 @@ const CMS_Menu cmsx_menuNavigation = {
.onGlobalExit = NULL, .onGlobalExit = NULL,
.entries = cmsx_menuNavigationEntries .entries = cmsx_menuNavigationEntries
}; };
#endif

View file

@ -30,7 +30,7 @@
#define IRLOCK_FRAME_SYNC ((uint32_t)(IRLOCK_OBJECT_SYNC | (IRLOCK_OBJECT_SYNC << 16))) #define IRLOCK_FRAME_SYNC ((uint32_t)(IRLOCK_OBJECT_SYNC | (IRLOCK_OBJECT_SYNC << 16)))
#if defined(USE_NAV) && defined(USE_IRLOCK) #if defined(USE_IRLOCK)
static bool irlockHealthy = false; static bool irlockHealthy = false;

View file

@ -20,7 +20,7 @@
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/io_types.h" #include "drivers/io_types.h"
#if defined(USE_NAV) && defined(USE_IRLOCK) #if defined(USE_IRLOCK)
#define IRLOCK_RES_X 320 #define IRLOCK_RES_X 320
#define IRLOCK_RES_Y 200 #define IRLOCK_RES_Y 200

View file

@ -1351,7 +1351,7 @@ static void cliSafeHomes(char *cmdline)
} }
#endif #endif
#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
{ {
cliPrintLinef("#wp %d %svalid", posControl.waypointCount, posControl.waypointListValid ? "" : "in"); //int8_t bool cliPrintLinef("#wp %d %svalid", posControl.waypointCount, posControl.waypointListValid ? "" : "in"); //int8_t bool
@ -3691,7 +3691,7 @@ static void printConfig(const char *cmdline, bool doDiff)
printTempSensor(dumpMask, tempSensorConfig_CopyArray, tempSensorConfig(0)); printTempSensor(dumpMask, tempSensorConfig_CopyArray, tempSensorConfig(0));
#endif #endif
#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
cliPrintHashLine("wp"); cliPrintHashLine("wp");
printWaypoints(dumpMask, posControl.waypointList, nonVolatileWaypointList(0)); printWaypoints(dumpMask, posControl.waypointList, nonVolatileWaypointList(0));
#endif #endif
@ -3932,7 +3932,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor), CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor),
#endif #endif
CLI_COMMAND_DEF("version", "show version", NULL, cliVersion), CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI)
CLI_COMMAND_DEF("wp", "waypoint list", NULL, cliWaypoints), CLI_COMMAND_DEF("wp", "waypoint list", NULL, cliWaypoints),
#endif #endif
#ifdef USE_OSD #ifdef USE_OSD

View file

@ -144,13 +144,11 @@ PG_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig,
} }
); );
#ifdef USE_NAV
void validateNavConfig(void) void validateNavConfig(void)
{ {
// Make sure minAlt is not more than maxAlt, maxAlt cannot be set lower than 500. // Make sure minAlt is not more than maxAlt, maxAlt cannot be set lower than 500.
navConfigMutable()->general.land_slowdown_minalt = MIN(navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt - 100); navConfigMutable()->general.land_slowdown_minalt = MIN(navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt - 100);
} }
#endif
// Stubs to handle target-specific configs // Stubs to handle target-specific configs
@ -232,10 +230,8 @@ void validateAndFixConfig(void)
pgResetCopy(serialConfigMutable(), PG_SERIAL_CONFIG); pgResetCopy(serialConfigMutable(), PG_SERIAL_CONFIG);
} }
#if defined(USE_NAV)
// Ensure sane values of navConfig settings // Ensure sane values of navConfig settings
validateNavConfig(); validateNavConfig();
#endif
// Limitations of different protocols // Limitations of different protocols
#if !defined(USE_DSHOT) #if !defined(USE_DSHOT)
@ -352,9 +348,7 @@ static void activateConfig(void)
pidInit(); pidInit();
#ifdef USE_NAV
navigationUsePIDs(); navigationUsePIDs();
#endif
} }
void readEEPROM(void) void readEEPROM(void)

View file

@ -158,11 +158,9 @@ bool areSensorsCalibrating(void)
} }
#endif #endif
#ifdef USE_NAV
if (!navIsCalibrationComplete()) { if (!navIsCalibrationComplete()) {
return true; return true;
} }
#endif
if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) { if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) {
return true; return true;
@ -248,7 +246,6 @@ static void updateArmingStatus(void)
DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED); DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED);
} }
#if defined(USE_NAV)
/* CHECK: Navigation safety */ /* CHECK: Navigation safety */
if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) { if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE); ENABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE);
@ -256,7 +253,6 @@ static void updateArmingStatus(void)
else { else {
DISABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE); DISABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE);
} }
#endif
#if defined(USE_MAG) #if defined(USE_MAG)
/* CHECK: */ /* CHECK: */
@ -552,7 +548,6 @@ void tryArm(void)
return; return;
} }
#if defined(USE_NAV)
// If nav_extra_arming_safety was bypassed we always // If nav_extra_arming_safety was bypassed we always
// allow bypassing it even without the sticks set // allow bypassing it even without the sticks set
// in the correct position to allow re-arming quickly // in the correct position to allow re-arming quickly
@ -562,7 +557,6 @@ void tryArm(void)
if (usedBypass) { if (usedBypass) {
ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED);
} }
#endif
lastDisarmReason = DISARM_NONE; lastDisarmReason = DISARM_NONE;
@ -591,15 +585,11 @@ void tryArm(void)
#endif #endif
//beep to indicate arming //beep to indicate arming
#ifdef USE_NAV
if (navigationPositionEstimateIsHealthy()) { if (navigationPositionEstimateIsHealthy()) {
beeper(BEEPER_ARMING_GPS_FIX); beeper(BEEPER_ARMING_GPS_FIX);
} else { } else {
beeper(BEEPER_ARMING); beeper(BEEPER_ARMING);
} }
#else
beeper(BEEPER_ARMING);
#endif
statsOnArm(); statsOnArm();
@ -865,11 +855,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
cycleTime = getTaskDeltaTime(TASK_SELF); cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f; dT = (float)cycleTime * 0.000001f;
#if defined(USE_NAV)
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) { if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) {
#else
if (ARMING_FLAG(ARMED)) {
#endif
flightTime += cycleTime; flightTime += cycleTime;
armTime += cycleTime; armTime += cycleTime;
updateAccExtremes(); updateAccExtremes();
@ -889,18 +875,14 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
rcInterpolationApply(isRXDataNew); rcInterpolationApply(isRXDataNew);
} }
#if defined(USE_NAV)
if (isRXDataNew) { if (isRXDataNew) {
updateWaypointsAndNavigationMode(); updateWaypointsAndNavigationMode();
} }
#endif
isRXDataNew = false; isRXDataNew = false;
#if defined(USE_NAV)
updatePositionEstimator(); updatePositionEstimator();
applyWaypointNavigationAndAltitudeHold(); applyWaypointNavigationAndAltitudeHold();
#endif
// Apply throttle tilt compensation // Apply throttle tilt compensation
if (!STATE(FIXED_WING_LEGACY)) { if (!STATE(FIXED_WING_LEGACY)) {

View file

@ -581,9 +581,7 @@ void init(void)
#endif #endif
#ifdef USE_NAV
navigationInit(); navigationInit();
#endif
#ifdef USE_LED_STRIP #ifdef USE_LED_STRIP
ledStripInit(); ledStripInit();

View file

@ -607,13 +607,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_ALTITUDE: case MSP_ALTITUDE:
#if defined(USE_NAV)
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z))); sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z))); sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
#else
sbufWriteU32(dst, 0);
sbufWriteU16(dst, 0);
#endif
#if defined(USE_BARO) #if defined(USE_BARO)
sbufWriteU32(dst, baroGetLatestAltitude()); sbufWriteU32(dst, baroGetLatestAltitude());
#else #else
@ -923,7 +918,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, GPS_directionToHome); sbufWriteU16(dst, GPS_directionToHome);
sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0); sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
break; break;
#ifdef USE_NAV
case MSP_NAV_STATUS: case MSP_NAV_STATUS:
sbufWriteU8(dst, NAV_Status.mode); sbufWriteU8(dst, NAV_Status.mode);
sbufWriteU8(dst, NAV_Status.state); sbufWriteU8(dst, NAV_Status.state);
@ -933,7 +927,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
//sbufWriteU16(dst, (int16_t)(target_bearing/100)); //sbufWriteU16(dst, (int16_t)(target_bearing/100));
sbufWriteU16(dst, getHeadingHoldTarget()); sbufWriteU16(dst, getHeadingHoldTarget());
break; break;
#endif
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
/* Compatibility stub - return zero SVs */ /* Compatibility stub - return zero SVs */
@ -1315,7 +1309,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif #endif
break; break;
#ifdef USE_NAV
case MSP_NAV_POSHOLD: case MSP_NAV_POSHOLD:
sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
sbufWriteU16(dst, navConfig()->general.max_auto_speed); sbufWriteU16(dst, navConfig()->general.max_auto_speed);
@ -1353,7 +1346,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle); sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle);
sbufWriteU16(dst, navConfig()->fw.loiter_radius); sbufWriteU16(dst, navConfig()->fw.loiter_radius);
break; break;
#endif
case MSP_CALIBRATION_DATA: case MSP_CALIBRATION_DATA:
sbufWriteU8(dst, accGetCalibrationAxisFlags()); sbufWriteU8(dst, accGetCalibrationAxisFlags());
@ -1393,7 +1385,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_POSITION_ESTIMATION_CONFIG: case MSP_POSITION_ESTIMATION_CONFIG:
#ifdef USE_NAV
sbufWriteU16(dst, positionEstimationConfig()->w_z_baro_p * 100); // inav_w_z_baro_p float as value * 100 sbufWriteU16(dst, positionEstimationConfig()->w_z_baro_p * 100); // inav_w_z_baro_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_p * 100); // 2 inav_w_z_gps_p float as value * 100 sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_p * 100); // 2 inav_w_z_gps_p float as value * 100
@ -1403,15 +1394,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, gpsConfigMutable()->gpsMinSats); // 1 sbufWriteU8(dst, gpsConfigMutable()->gpsMinSats); // 1
sbufWriteU8(dst, positionEstimationConfig()->use_gps_velned); // 1 inav_use_gps_velned ON/OFF sbufWriteU8(dst, positionEstimationConfig()->use_gps_velned); // 1 inav_use_gps_velned ON/OFF
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
break; break;
case MSP_REBOOT: case MSP_REBOOT:
@ -1423,17 +1405,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_WP_GETINFO: case MSP_WP_GETINFO:
#ifdef USE_NAV
sbufWriteU8(dst, 0); // Reserved for waypoint capabilities sbufWriteU8(dst, 0); // Reserved for waypoint capabilities
sbufWriteU8(dst, NAV_MAX_WAYPOINTS); // Maximum number of waypoints supported sbufWriteU8(dst, NAV_MAX_WAYPOINTS); // Maximum number of waypoints supported
sbufWriteU8(dst, isWaypointListValid()); // Is current mission valid sbufWriteU8(dst, isWaypointListValid()); // Is current mission valid
sbufWriteU8(dst, getWaypointCount()); // Number of waypoints in current mission sbufWriteU8(dst, getWaypointCount()); // Number of waypoints in current mission
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
break; break;
case MSP_TX_INFO: case MSP_TX_INFO:
@ -1622,7 +1597,6 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
} }
} }
#ifdef USE_NAV
static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src) static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
{ {
const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number
@ -1638,7 +1612,6 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
sbufWriteU16(dst, msp_wp.p3); // P3 sbufWriteU16(dst, msp_wp.p3); // P3
sbufWriteU8(dst, msp_wp.flag); // flags sbufWriteU8(dst, msp_wp.flag); // flags
} }
#endif
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
@ -1728,9 +1701,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
pidBankMutable()->pid[i].D = sbufReadU8(src); pidBankMutable()->pid[i].D = sbufReadU8(src);
} }
schedulePidGainsUpdate(); schedulePidGainsUpdate();
#if defined(USE_NAV)
navigationUsePIDs(); navigationUsePIDs();
#endif
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -1744,9 +1715,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
pidBankMutable()->pid[i].FF = sbufReadU8(src); pidBankMutable()->pid[i].FF = sbufReadU8(src);
} }
schedulePidGainsUpdate(); schedulePidGainsUpdate();
#if defined(USE_NAV)
navigationUsePIDs(); navigationUsePIDs();
#endif
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -2284,7 +2253,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
#ifdef USE_NAV
case MSP_SET_NAV_POSHOLD: case MSP_SET_NAV_POSHOLD:
if (dataSize >= 13) { if (dataSize >= 13) {
navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
@ -2331,7 +2299,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
#endif
case MSP_SET_CALIBRATION_DATA: case MSP_SET_CALIBRATION_DATA:
if (dataSize >= 18) { if (dataSize >= 18) {
@ -2373,7 +2340,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
#ifdef USE_NAV
case MSP_SET_POSITION_ESTIMATION_CONFIG: case MSP_SET_POSITION_ESTIMATION_CONFIG:
if (dataSize >= 12) { if (dataSize >= 12) {
positionEstimationConfigMutable()->w_z_baro_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f); positionEstimationConfigMutable()->w_z_baro_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
@ -2386,7 +2352,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
#endif
case MSP_RESET_CONF: case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
@ -2576,7 +2541,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break; break;
#endif #endif
#ifdef USE_NAV
case MSP_SET_WP: case MSP_SET_WP:
if (dataSize >= 21) { if (dataSize >= 21) {
const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
@ -2606,7 +2570,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
#endif
case MSP_SET_FEATURE: case MSP_SET_FEATURE:
if (dataSize >= 4) { if (dataSize >= 4) {
@ -3314,12 +3277,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
{ {
switch (cmdMSP) { switch (cmdMSP) {
#if defined(USE_NAV)
case MSP_WP: case MSP_WP:
mspFcWaypointOutCommand(dst, src); mspFcWaypointOutCommand(dst, src);
*ret = MSP_RESULT_ACK; *ret = MSP_RESULT_ACK;
break; break;
#endif
#if defined(USE_FLASHFS) #if defined(USE_FLASHFS)
case MSP_DATAFLASH_READ: case MSP_DATAFLASH_READ:

View file

@ -225,7 +225,7 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
} }
#endif #endif
#if defined(USE_NAV) && defined(USE_IRLOCK) #if defined(USE_IRLOCK)
void taskUpdateIrlock(timeUs_t currentTimeUs) void taskUpdateIrlock(timeUs_t currentTimeUs)
{ {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);

View file

@ -251,7 +251,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
} }
#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
// Save waypoint list // Save waypoint list
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
const bool success = saveNonVolatileWaypointList(); const bool success = saveNonVolatileWaypointList();

View file

@ -39,9 +39,7 @@
#include "rx/rx.h" #include "rx/rx.h"
static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT]; static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
#ifdef USE_NAV
static bool isUsingNAVModes = false; static bool isUsingNAVModes = false;
#endif
boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
@ -119,13 +117,10 @@ void processAirmode(void) {
} }
#if defined(USE_NAV)
bool isUsingNavigationModes(void) bool isUsingNavigationModes(void)
{ {
return isUsingNAVModes; return isUsingNAVModes;
} }
#endif
bool IS_RC_MODE_ACTIVE(boxId_e boxId) bool IS_RC_MODE_ACTIVE(boxId_e boxId)
{ {
@ -208,13 +203,11 @@ void updateUsedModeActivationConditionFlags(void)
} }
} }
#ifdef USE_NAV
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
isModeActivationConditionPresent(BOXNAVRTH) || isModeActivationConditionPresent(BOXNAVRTH) ||
isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
isModeActivationConditionPresent(BOXNAVCRUISE) || isModeActivationConditionPresent(BOXNAVCRUISE) ||
isModeActivationConditionPresent(BOXNAVWP); isModeActivationConditionPresent(BOXNAVWP);
#endif
} }
void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm) void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm)

View file

@ -1885,7 +1885,6 @@ groups:
description: "Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick." description: "Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick."
default_value: "RIGHT" default_value: "RIGHT"
field: loiter_direction field: loiter_direction
condition: USE_NAV
table: direction table: direction
- name: fw_reference_airspeed - name: fw_reference_airspeed
description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
@ -1954,21 +1953,18 @@ groups:
- name: nav_mc_pos_z_p - name: nav_mc_pos_z_p
description: "P gain of altitude PID controller (Multirotor)" description: "P gain of altitude PID controller (Multirotor)"
field: bank_mc.pid[PID_POS_Z].P field: bank_mc.pid[PID_POS_Z].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 50 default_value: 50
- name: nav_mc_vel_z_p - name: nav_mc_vel_z_p
description: "P gain of velocity PID controller" description: "P gain of velocity PID controller"
field: bank_mc.pid[PID_VEL_Z].P field: bank_mc.pid[PID_VEL_Z].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 100 default_value: 100
- name: nav_mc_vel_z_i - name: nav_mc_vel_z_i
description: "I gain of velocity PID controller" description: "I gain of velocity PID controller"
field: bank_mc.pid[PID_VEL_Z].I field: bank_mc.pid[PID_VEL_Z].I
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 50 default_value: 50
@ -1976,41 +1972,35 @@ groups:
description: "D gain of velocity PID controller" description: "D gain of velocity PID controller"
default_value: 10 default_value: 10
field: bank_mc.pid[PID_VEL_Z].D field: bank_mc.pid[PID_VEL_Z].D
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 10 default_value: 10
- name: nav_mc_pos_xy_p - name: nav_mc_pos_xy_p
description: "Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity" description: "Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity"
field: bank_mc.pid[PID_POS_XY].P field: bank_mc.pid[PID_POS_XY].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 65 default_value: 65
- name: nav_mc_vel_xy_p - name: nav_mc_vel_xy_p
description: "P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause \"nervous\" behavior and oscillations" description: "P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause \"nervous\" behavior and oscillations"
field: bank_mc.pid[PID_VEL_XY].P field: bank_mc.pid[PID_VEL_XY].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 40 default_value: 40
- name: nav_mc_vel_xy_i - name: nav_mc_vel_xy_i
description: "I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot" description: "I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot"
field: bank_mc.pid[PID_VEL_XY].I field: bank_mc.pid[PID_VEL_XY].I
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 15 default_value: 15
- name: nav_mc_vel_xy_d - name: nav_mc_vel_xy_d
description: "D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target." description: "D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target."
field: bank_mc.pid[PID_VEL_XY].D field: bank_mc.pid[PID_VEL_XY].D
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 100 default_value: 100
- name: nav_mc_vel_xy_ff - name: nav_mc_vel_xy_ff
field: bank_mc.pid[PID_VEL_XY].FF field: bank_mc.pid[PID_VEL_XY].FF
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
default_value: 40 default_value: 40
@ -2018,7 +2008,6 @@ groups:
description: "P gain of Heading Hold controller (Multirotor)" description: "P gain of Heading Hold controller (Multirotor)"
default_value: 60 default_value: 60
field: bank_mc.pid[PID_HEADING].P field: bank_mc.pid[PID_HEADING].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_mc_vel_xy_dterm_lpf_hz - name: nav_mc_vel_xy_dterm_lpf_hz
@ -2048,70 +2037,60 @@ groups:
description: "P gain of altitude PID controller (Fixedwing)" description: "P gain of altitude PID controller (Fixedwing)"
default_value: 40 default_value: 40
field: bank_fw.pid[PID_POS_Z].P field: bank_fw.pid[PID_POS_Z].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_z_i - name: nav_fw_pos_z_i
description: "I gain of altitude PID controller (Fixedwing)" description: "I gain of altitude PID controller (Fixedwing)"
default_value: 5 default_value: 5
field: bank_fw.pid[PID_POS_Z].I field: bank_fw.pid[PID_POS_Z].I
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_z_d - name: nav_fw_pos_z_d
description: "D gain of altitude PID controller (Fixedwing)" description: "D gain of altitude PID controller (Fixedwing)"
default_value: 10 default_value: 10
field: bank_fw.pid[PID_POS_Z].D field: bank_fw.pid[PID_POS_Z].D
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_xy_p - name: nav_fw_pos_xy_p
description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH"
default_value: 75 default_value: 75
field: bank_fw.pid[PID_POS_XY].P field: bank_fw.pid[PID_POS_XY].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_xy_i - name: nav_fw_pos_xy_i
description: "I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero" description: "I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero"
default_value: 5 default_value: 5
field: bank_fw.pid[PID_POS_XY].I field: bank_fw.pid[PID_POS_XY].I
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_xy_d - name: nav_fw_pos_xy_d
description: "D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero" description: "D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero"
default_value: 8 default_value: 8
field: bank_fw.pid[PID_POS_XY].D field: bank_fw.pid[PID_POS_XY].D
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_heading_p - name: nav_fw_heading_p
description: "P gain of Heading Hold controller (Fixedwing)" description: "P gain of Heading Hold controller (Fixedwing)"
default_value: 60 default_value: 60
field: bank_fw.pid[PID_HEADING].P field: bank_fw.pid[PID_HEADING].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_hdg_p - name: nav_fw_pos_hdg_p
description: "P gain of heading PID controller. (Fixedwing, rovers, boats)" description: "P gain of heading PID controller. (Fixedwing, rovers, boats)"
default_value: 30 default_value: 30
field: bank_fw.pid[PID_POS_HEADING].P field: bank_fw.pid[PID_POS_HEADING].P
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_hdg_i - name: nav_fw_pos_hdg_i
description: "I gain of heading trajectory PID controller. (Fixedwing, rovers, boats)" description: "I gain of heading trajectory PID controller. (Fixedwing, rovers, boats)"
default_value: 2 default_value: 2
field: bank_fw.pid[PID_POS_HEADING].I field: bank_fw.pid[PID_POS_HEADING].I
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_hdg_d - name: nav_fw_pos_hdg_d
description: "D gain of heading trajectory PID controller. (Fixedwing, rovers, boats)" description: "D gain of heading trajectory PID controller. (Fixedwing, rovers, boats)"
default_value: 0 default_value: 0
field: bank_fw.pid[PID_POS_HEADING].D field: bank_fw.pid[PID_POS_HEADING].D
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_fw_pos_hdg_pidsum_limit - name: nav_fw_pos_hdg_pidsum_limit
@ -2244,7 +2223,6 @@ groups:
- name: PG_POSITION_ESTIMATION_CONFIG - name: PG_POSITION_ESTIMATION_CONFIG
type: positionEstimationConfig_t type: positionEstimationConfig_t
condition: USE_NAV
members: members:
- name: inav_auto_mag_decl - name: inav_auto_mag_decl
description: "Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored." description: "Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored."
@ -2376,7 +2354,6 @@ groups:
- name: PG_NAV_CONFIG - name: PG_NAV_CONFIG
type: navConfig_t type: navConfig_t
headers: ["navigation/navigation.h"] headers: ["navigation/navigation.h"]
condition: USE_NAV
members: members:
- name: nav_disarm_on_landing - name: nav_disarm_on_landing
description: "If set to ON, iNav disarms the FC after landing" description: "If set to ON, iNav disarms the FC after landing"
@ -2669,14 +2646,12 @@ groups:
description: "Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting" description: "Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting"
default_value: 120 default_value: 120
field: mc.posDecelerationTime field: mc.posDecelerationTime
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_mc_pos_expo - name: nav_mc_pos_expo
description: "Expo for PosHold control" description: "Expo for PosHold control"
default_value: 10 default_value: 10
field: mc.posResponseExpo field: mc.posResponseExpo
condition: USE_NAV
min: 0 min: 0
max: 255 max: 255
- name: nav_mc_wp_slowdown - name: nav_mc_wp_slowdown

View file

@ -87,9 +87,6 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
typedef enum { typedef enum {
FAILSAFE_CHANNEL_HOLD, // Hold last known good value FAILSAFE_CHANNEL_HOLD, // Hold last known good value
FAILSAFE_CHANNEL_NEUTRAL, // RPY = zero, THR = zero FAILSAFE_CHANNEL_NEUTRAL, // RPY = zero, THR = zero
#if !defined(USE_NAV)
FAILSAFE_CHANNEL_AUTO, // Defined by failsafe configured values
#endif
} failsafeChannelBehavior_e; } failsafeChannelBehavior_e;
typedef struct { typedef struct {
@ -101,7 +98,6 @@ typedef struct {
static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
[FAILSAFE_PROCEDURE_AUTO_LANDING] = { [FAILSAFE_PROCEDURE_AUTO_LANDING] = {
.forceAngleMode = true, .forceAngleMode = true,
#if defined(USE_NAV)
.bypassNavigation = false, .bypassNavigation = false,
.channelBehavior = { .channelBehavior = {
FAILSAFE_CHANNEL_NEUTRAL, // ROLL FAILSAFE_CHANNEL_NEUTRAL, // ROLL
@ -109,15 +105,6 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
FAILSAFE_CHANNEL_NEUTRAL, // YAW FAILSAFE_CHANNEL_NEUTRAL, // YAW
FAILSAFE_CHANNEL_HOLD // THROTTLE FAILSAFE_CHANNEL_HOLD // THROTTLE
} }
#else
.bypassNavigation = true,
.channelBehavior = {
FAILSAFE_CHANNEL_AUTO, // ROLL
FAILSAFE_CHANNEL_AUTO, // PITCH
FAILSAFE_CHANNEL_AUTO, // YAW
FAILSAFE_CHANNEL_AUTO // THROTTLE
}
#endif
}, },
[FAILSAFE_PROCEDURE_DROP_IT] = { [FAILSAFE_PROCEDURE_DROP_IT] = {
@ -184,7 +171,6 @@ void failsafeInit(void)
failsafeState.suspended = false; failsafeState.suspended = false;
} }
#ifdef USE_NAV
bool failsafeBypassNavigation(void) bool failsafeBypassNavigation(void)
{ {
return failsafeState.active && return failsafeState.active &&
@ -197,7 +183,6 @@ bool failsafeMayRequireNavigationMode(void)
return (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH) || return (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH) ||
(failsafeConfig()->failsafe_min_distance_procedure == FAILSAFE_PROCEDURE_RTH); (failsafeConfig()->failsafe_min_distance_procedure == FAILSAFE_PROCEDURE_RTH);
} }
#endif
failsafePhase_e failsafePhase(void) failsafePhase_e failsafePhase(void)
{ {
@ -228,16 +213,10 @@ bool failsafeRequiresAngleMode(void)
bool failsafeRequiresMotorStop(void) bool failsafeRequiresMotorStop(void)
{ {
#if defined(USE_NAV)
return failsafeState.active && return failsafeState.active &&
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
posControl.flags.estAltStatus < EST_USABLE && posControl.flags.estAltStatus < EST_USABLE &&
currentBatteryProfile->failsafe_throttle < getThrottleIdleValue(); currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
#else
return failsafeState.active &&
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
#endif
} }
void failsafeStartMonitoring(void) void failsafeStartMonitoring(void)
@ -277,22 +256,6 @@ void failsafeUpdateRcCommandValues(void)
void failsafeApplyControlInput(void) void failsafeApplyControlInput(void)
{ {
#if !defined(USE_NAV)
// Prepare FAILSAFE_CHANNEL_AUTO values for rcCommand
int16_t autoRcCommand[4];
if (STATE(FIXED_WING_LEGACY)) {
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
}
else {
for (int i = 0; i < 3; i++) {
autoRcCommand[i] = 0;
}
autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
}
#endif
// Apply channel values // Apply channel values
for (int idx = 0; idx < 4; idx++) { for (int idx = 0; idx < 4; idx++) {
switch (failsafeProcedureLogic[failsafeState.activeProcedure].channelBehavior[idx]) { switch (failsafeProcedureLogic[failsafeState.activeProcedure].channelBehavior[idx]) {
@ -313,11 +276,6 @@ void failsafeApplyControlInput(void)
break; break;
} }
break; break;
#if !defined(USE_NAV)
case FAILSAFE_CHANNEL_AUTO:
rcCommand[idx] = autoRcCommand[idx];
break;
#endif
} }
} }
} }
@ -461,9 +419,7 @@ void failsafeUpdateState(void)
case FAILSAFE_PROCEDURE_AUTO_LANDING: case FAILSAFE_PROCEDURE_AUTO_LANDING:
// Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level). // Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level).
failsafeActivate(FAILSAFE_LANDING); failsafeActivate(FAILSAFE_LANDING);
#if defined(USE_NAV)
activateForcedEmergLanding(); activateForcedEmergLanding();
#endif
break; break;
case FAILSAFE_PROCEDURE_DROP_IT: case FAILSAFE_PROCEDURE_DROP_IT:
@ -472,13 +428,11 @@ void failsafeUpdateState(void)
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
break; break;
#if defined(USE_NAV)
case FAILSAFE_PROCEDURE_RTH: case FAILSAFE_PROCEDURE_RTH:
// Proceed to handling & monitoring RTH navigation // Proceed to handling & monitoring RTH navigation
failsafeActivate(FAILSAFE_RETURN_TO_HOME); failsafeActivate(FAILSAFE_RETURN_TO_HOME);
activateForcedRTH(); activateForcedRTH();
break; break;
#endif
case FAILSAFE_PROCEDURE_NONE: case FAILSAFE_PROCEDURE_NONE:
default: default:
// Do nothing procedure // Do nothing procedure
@ -497,7 +451,6 @@ void failsafeUpdateState(void)
} }
break; break;
#if defined(USE_NAV)
case FAILSAFE_RETURN_TO_HOME: case FAILSAFE_RETURN_TO_HOME:
if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
abortForcedRTH(); abortForcedRTH();
@ -533,20 +486,16 @@ void failsafeUpdateState(void)
} }
} }
break; break;
#endif
case FAILSAFE_LANDING: case FAILSAFE_LANDING:
if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
#if defined(USE_NAV)
abortForcedEmergLanding(); abortForcedEmergLanding();
#endif
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true; reprocessState = true;
} else { } else {
if (armed) { if (armed) {
beeperMode = BEEPER_RX_LOST_LANDING; beeperMode = BEEPER_RX_LOST_LANDING;
} }
#if defined(USE_NAV)
bool emergLanded = false; bool emergLanded = false;
switch (getStateOfForcedEmergLanding()) { switch (getStateOfForcedEmergLanding()) {
case EMERG_LAND_IN_PROGRESS: case EMERG_LAND_IN_PROGRESS:
@ -566,9 +515,6 @@ void failsafeUpdateState(void)
break; break;
} }
if (emergLanded || failsafeShouldHaveCausedLandingByNow() || !armed) { if (emergLanded || failsafeShouldHaveCausedLandingByNow() || !armed) {
#else
if (failsafeShouldHaveCausedLandingByNow() || !armed) {
#endif
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
failsafeState.phase = FAILSAFE_LANDED; failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true; reprocessState = true;

View file

@ -70,7 +70,6 @@ typedef enum {
* Note that this phase is only used when * Note that this phase is only used when
* failsafe_procedure = NONE. * failsafe_procedure = NONE.
*/ */
#if defined(USE_NAV)
FAILSAFE_RETURN_TO_HOME, FAILSAFE_RETURN_TO_HOME,
/* Failsafe is executing RTH. This phase is the first one /* Failsafe is executing RTH. This phase is the first one
* enabled when failsafe_procedure = RTH if an RTH is * enabled when failsafe_procedure = RTH if an RTH is
@ -79,10 +78,10 @@ typedef enum {
* sensors are not working at the moment). If RTH can't * sensors are not working at the moment). If RTH can't
* be started, this phase will transition to FAILSAFE_LANDING. * be started, this phase will transition to FAILSAFE_LANDING.
*/ */
#endif
FAILSAFE_LANDING, FAILSAFE_LANDING,
/* Pergorms NAV Emergency Landing if USE_NAV defined. /* Performs NAV Emergency Landing using controlled descent rate if
* Otherwise Failsafe mode performs a simplified landing procedure. * altitude sensors available.
* Otherwise Emergency Landing performs a simplified landing procedure.
* This is done by setting throttle and roll/pitch/yaw controls * This is done by setting throttle and roll/pitch/yaw controls
* to a pre-configured values that will allow aircraft * to a pre-configured values that will allow aircraft
* to reach ground in somewhat safe "controlled crash" way. * to reach ground in somewhat safe "controlled crash" way.
@ -123,7 +122,6 @@ typedef enum {
FAILSAFE_PROCEDURE_NONE FAILSAFE_PROCEDURE_NONE
} failsafeProcedure_e; } failsafeProcedure_e;
#if defined(USE_NAV)
typedef enum { typedef enum {
RTH_IDLE = 0, // RTH is waiting RTH_IDLE = 0, // RTH is waiting
RTH_IN_PROGRESS, // RTH is active RTH_IN_PROGRESS, // RTH is active
@ -135,7 +133,6 @@ typedef enum {
EMERG_LAND_IN_PROGRESS, // Emergency landing is active EMERG_LAND_IN_PROGRESS, // Emergency landing is active
EMERG_LAND_HAS_LANDED // Emergency landing is active and has landed. EMERG_LAND_HAS_LANDED // Emergency landing is active and has landed.
} emergLandState_e; } emergLandState_e;
#endif
typedef struct failsafeState_s { typedef struct failsafeState_s {
int16_t events; int16_t events;

View file

@ -892,7 +892,6 @@ static uint8_t getHeadingHoldState(void)
return HEADING_HOLD_DISABLED; return HEADING_HOLD_DISABLED;
} }
#if defined(USE_NAV)
int navHeadingState = navigationGetHeadingControlState(); int navHeadingState = navigationGetHeadingControlState();
// NAV will prevent MAG_MODE from activating, but require heading control // NAV will prevent MAG_MODE from activating, but require heading control
if (navHeadingState != NAV_HEADING_CONTROL_NONE) { if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
@ -901,9 +900,7 @@ static uint8_t getHeadingHoldState(void)
return HEADING_HOLD_ENABLED; return HEADING_HOLD_ENABLED;
} }
} }
else else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
#endif
if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
return HEADING_HOLD_ENABLED; return HEADING_HOLD_ENABLED;
} else { } else {
return HEADING_HOLD_UPDATE_HEADING; return HEADING_HOLD_UPDATE_HEADING;

View file

@ -264,11 +264,9 @@ static void updateFailsafeStatus(void)
case FAILSAFE_RX_LOSS_IDLE: case FAILSAFE_RX_LOSS_IDLE:
failsafeIndicator = 'I'; failsafeIndicator = 'I';
break; break;
#if defined(USE_NAV)
case FAILSAFE_RETURN_TO_HOME: case FAILSAFE_RETURN_TO_HOME:
failsafeIndicator = 'H'; failsafeIndicator = 'H';
break; break;
#endif
case FAILSAFE_LANDING: case FAILSAFE_LANDING:
failsafeIndicator = 'l'; failsafeIndicator = 'l';
break; break;

View file

@ -757,7 +757,6 @@ static const char * osdArmingDisabledReasonMessage(void)
case ARMING_DISABLED_SYSTEM_OVERLOADED: case ARMING_DISABLED_SYSTEM_OVERLOADED:
return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED); return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED);
case ARMING_DISABLED_NAVIGATION_UNSAFE: case ARMING_DISABLED_NAVIGATION_UNSAFE:
#if defined(USE_NAV)
// Check the exact reason // Check the exact reason
switch (navigationIsBlockingArming(NULL)) { switch (navigationIsBlockingArming(NULL)) {
char buf[6]; char buf[6];
@ -774,7 +773,6 @@ static const char * osdArmingDisabledReasonMessage(void)
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG);
} }
#endif
break; break;
case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL); return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL);
@ -850,11 +848,9 @@ static const char * osdFailsafePhaseMessage(void)
{ {
// See failsafe.h for each phase explanation // See failsafe.h for each phase explanation
switch (failsafePhase()) { switch (failsafePhase()) {
#ifdef USE_NAV
case FAILSAFE_RETURN_TO_HOME: case FAILSAFE_RETURN_TO_HOME:
// XXX: Keep this in sync with OSD_FLYMODE. // XXX: Keep this in sync with OSD_FLYMODE.
return OSD_MESSAGE_STR(OSD_MSG_RTH_FS); return OSD_MESSAGE_STR(OSD_MSG_RTH_FS);
#endif
case FAILSAFE_LANDING: case FAILSAFE_LANDING:
// This should be considered an emergengy landing // This should be considered an emergengy landing
return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS);
@ -1074,24 +1070,12 @@ static void osdFormatRpm(char *buff, uint32_t rpm)
int32_t osdGetAltitude(void) int32_t osdGetAltitude(void)
{ {
#if defined(USE_NAV)
return getEstimatedActualPosition(Z); return getEstimatedActualPosition(Z);
#elif defined(USE_BARO)
return baro.alt;
#else
return 0;
#endif
} }
static inline int32_t osdGetAltitudeMsl(void) static inline int32_t osdGetAltitudeMsl(void)
{ {
#if defined(USE_NAV)
return getEstimatedActualPosition(Z)+GPS_home.alt; return getEstimatedActualPosition(Z)+GPS_home.alt;
#elif defined(USE_BARO)
return baro.alt+GPS_home.alt;
#else
return 0;
#endif
} }
static bool osdIsHeadingValid(void) static bool osdIsHeadingValid(void)
@ -2975,7 +2959,6 @@ static bool osdDrawSingleElement(uint8_t item)
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS);
return true; return true;
#if defined(USE_NAV)
case OSD_MISSION: case OSD_MISSION:
{ {
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) { if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) {
@ -3022,7 +3005,6 @@ static bool osdDrawSingleElement(uint8_t item)
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
return true; return true;
} }
#endif // USE_NAV
#ifdef USE_POWER_LIMITS #ifdef USE_POWER_LIMITS
case OSD_PLIMIT_REMAINING_BURST_TIME: case OSD_PLIMIT_REMAINING_BURST_TIME:
@ -3870,7 +3852,6 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf ); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf );
y += 1; y += 1;
} }
#if defined(USE_NAV)
if (posControl.waypointListValid && posControl.waypointCount > 0) { if (posControl.waypointListValid && posControl.waypointCount > 0) {
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount);
@ -3879,7 +3860,6 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, 7, y, "*MISSION LOADED*"); displayWrite(osdDisplayPort, 7, y, "*MISSION LOADED*");
#endif #endif
} }
#endif
y += 1; y += 1;
#if defined(USE_GPS) #if defined(USE_GPS)
@ -4272,11 +4252,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (FLIGHT_MODE(SOARING_MODE)) { if (FLIGHT_MODE(SOARING_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING);
} }
#if defined(USE_NAV)
if (posControl.flags.wpMissionPlannerActive) { if (posControl.flags.wpMissionPlannerActive) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
} }
#endif
} }
} }
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {

View file

@ -1475,10 +1475,6 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src); pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src);
} }
schedulePidGainsUpdate(); schedulePidGainsUpdate();
#if defined(USE_NAV)
// This is currently unnecessary, DJI HD doesn't set any NAV PIDs
//navigationUsePIDs();
#endif
} }
else { else {
reply->result = MSP_RESULT_ERROR; reply->result = MSP_RESULT_ERROR;

View file

@ -90,8 +90,6 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO
#endif #endif
#if defined(USE_NAV)
// waypoint 254, 255 are special waypoints // waypoint 254, 255 are special waypoints
STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range); STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
@ -3987,70 +3985,3 @@ bool isAdjustingHeading(void) {
int32_t getCruiseHeadingAdjustment(void) { int32_t getCruiseHeadingAdjustment(void) {
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw); return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
} }
#else // NAV
#ifdef USE_GPS
/* Fallback if navigation is not compiled in - handle GPS home coordinates */
static float GPS_scaleLonDown;
static float GPS_totalTravelDistance = 0;
static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing)
{
const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
const float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown;
*dist = fast_fsqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
*bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
void onNewGPSData(void)
{
static timeMs_t previousTimeMs = 0;
const timeMs_t currentTimeMs = millis();
const timeDelta_t timeDeltaMs = currentTimeMs - previousTimeMs;
previousTimeMs = currentTimeMs;
if (!(sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5))
return;
if (ARMING_FLAG(ARMED)) {
/* Update home distance and direction */
if (STATE(GPS_FIX_HOME)) {
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(gpsSol.llh.lat, gpsSol.llh.lon, GPS_home.lat, GPS_home.lon, &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = lrintf(dir / 100.0f);
} else {
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
/* Update trip distance */
GPS_totalTravelDistance += gpsSol.groundSpeed * MS2S(timeDeltaMs);
}
else {
// Set home position to current GPS coordinates
ENABLE_STATE(GPS_FIX_HOME);
GPS_home.lat = gpsSol.llh.lat;
GPS_home.lon = gpsSol.llh.lon;
GPS_home.alt = gpsSol.llh.alt;
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
GPS_scaleLonDown = cos_approx((fabsf((float)gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f);
}
}
int32_t getTotalTravelDistance(void)
{
return lrintf(GPS_totalTravelDistance);
}
#endif
#endif // NAV

View file

@ -68,8 +68,6 @@ bool findNearestSafeHome(void); // Find nearest safehome
#endif // defined(USE_SAFE_HOME) #endif // defined(USE_SAFE_HOME)
#if defined(USE_NAV)
#ifndef NAV_MAX_WAYPOINTS #ifndef NAV_MAX_WAYPOINTS
#define NAV_MAX_WAYPOINTS 15 #define NAV_MAX_WAYPOINTS 15
#endif #endif
@ -601,15 +599,3 @@ extern uint16_t navFlags;
extern uint16_t navEPH; extern uint16_t navEPH;
extern uint16_t navEPV; extern uint16_t navEPV;
extern int16_t navAccNEU[3]; extern int16_t navAccNEU[3];
#else
#define navigationRequiresAngleMode() (0)
#define navigationGetHeadingControlState() (0)
#define navigationRequiresThrottleTiltCompensation() (0)
#define getEstimatedActualVelocity(axis) (0)
#define navigationIsControllingThrottle() (0)
#define navigationRTHAllowsLanding() (0)
#define navigationGetHomeHeading() (0)
#endif

View file

@ -21,8 +21,6 @@
#include "platform.h" #include "platform.h"
#if defined(USE_NAV)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
@ -699,5 +697,3 @@ int32_t navigationGetHeadingError(void)
{ {
return navHeadingError; return navHeadingError;
} }
#endif // NAV

View file

@ -22,8 +22,6 @@
#include "platform.h" #include "platform.h"
#if defined(USE_NAV)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
@ -540,5 +538,3 @@ const char * fixedWingLaunchStateMessage(void)
return NULL; return NULL;
} }
} }
#endif

View file

@ -21,8 +21,6 @@
#include "platform.h" #include "platform.h"
#if defined(USE_NAV)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
@ -169,6 +167,3 @@ bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t * origin, c
llh->alt += lrintf(pos->z); llh->alt += lrintf(pos->z);
return origin->valid; return origin->valid;
} }
#endif // NAV

View file

@ -21,8 +21,6 @@
#include "platform.h" #include "platform.h"
#if defined(USE_NAV)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
@ -819,4 +817,3 @@ void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlag
applyMulticopterHeadingController(); applyMulticopterHeadingController();
} }
} }
#endif // NAV

View file

@ -22,8 +22,6 @@
#include "platform.h" #include "platform.h"
#if defined(USE_NAV)
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
@ -861,5 +859,3 @@ bool navIsCalibrationComplete(void)
{ {
return gravityCalibrationComplete(); return gravityCalibrationComplete();
} }
#endif

View file

@ -32,8 +32,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#if defined(USE_NAV)
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "navigation/navigation_pos_estimator_private.h" #include "navigation/navigation_pos_estimator_private.h"
@ -202,5 +200,3 @@ void estimationCalculateAGL(estimationContext_t * ctx)
posEstimator.est.aglQual = SURFACE_QUAL_LOW; posEstimator.est.aglQual = SURFACE_QUAL_LOW;
#endif #endif
} }
#endif // NAV

View file

@ -32,8 +32,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#if defined(USE_NAV)
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "navigation/navigation_pos_estimator_private.h" #include "navigation/navigation_pos_estimator_private.h"
@ -123,6 +121,3 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
return false; return false;
#endif #endif
} }
#endif // NAV

View file

@ -19,8 +19,6 @@
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM
#if defined(USE_NAV)
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/filter.h" #include "common/filter.h"
@ -479,5 +477,3 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs);
* Rover specific functions * Rover specific functions
*/ */
void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
#endif

View file

@ -26,8 +26,6 @@
FILE_COMPILE_FOR_SIZE FILE_COMPILE_FOR_SIZE
#ifdef USE_NAV
#include "build/debug.h" #include "build/debug.h"
#include "common/utils.h" #include "common/utils.h"
@ -148,5 +146,3 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs); applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
} }
} }
#endif

View file

@ -349,11 +349,7 @@ static void telemetryRX(void)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
uint16_t gpsspeed = (gpsSol.groundSpeed*9L)/250L; uint16_t gpsspeed = (gpsSol.groundSpeed*9L)/250L;
int16_t course = (gpsSol.groundCourse/10 + 360)%360; int16_t course = (gpsSol.groundCourse/10 + 360)%360;
#ifdef USE_NAV
int32_t alt = getEstimatedActualPosition(Z); int32_t alt = getEstimatedActualPosition(Z);
#else
int32_t alt = baro.BaroAlt;
#endif
uint16_t tim = 0; uint16_t tim = 0;
rfTxBuffer[0] = 0x47; rfTxBuffer[0] = 0x47;

View file

@ -44,7 +44,7 @@
#define IRLOCK_TIMEOUT 100 #define IRLOCK_TIMEOUT 100
#if defined(USE_NAV) && defined(USE_IRLOCK) #if defined(USE_IRLOCK)
enum { enum {
DEBUG_IRLOCK_DETECTED, DEBUG_IRLOCK_DETECTED,

View file

@ -17,7 +17,7 @@
#pragma once #pragma once
#if defined(USE_NAV) && defined(USE_IRLOCK) #if defined(USE_IRLOCK)
void irlockInit(void); void irlockInit(void);
bool irlockHasBeenDetected(void); bool irlockHasBeenDetected(void);

View file

@ -141,7 +141,6 @@
#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
#define USE_NAV
#define NAV_GPS_GLITCH_DETECTION #define NAV_GPS_GLITCH_DETECTION
#define USE_ADC #define USE_ADC

View file

@ -150,7 +150,6 @@
// #define USE_RANGEFINDER_SRF10 // #define USE_RANGEFINDER_SRF10
// *************** NAV ***************************** // *************** NAV *****************************
#define USE_NAV
#define NAV_GPS_GLITCH_DETECTION #define NAV_GPS_GLITCH_DETECTION
#define NAV_MAX_WAYPOINTS 60 #define NAV_MAX_WAYPOINTS 60

View file

@ -66,7 +66,6 @@
#define USE_GPS #define USE_GPS
#define USE_GPS_PROTO_UBLOX #define USE_GPS_PROTO_UBLOX
#define USE_GPS_PROTO_MSP #define USE_GPS_PROTO_MSP
#define USE_NAV
#define USE_TELEMETRY #define USE_TELEMETRY
#define USE_TELEMETRY_LTM #define USE_TELEMETRY_LTM
#define USE_TELEMETRY_FRSKY #define USE_TELEMETRY_FRSKY

View file

@ -260,12 +260,10 @@ void initJetiExBusTelemetry(void)
bitArraySet(&exSensorEnabled, EX_POWER); bitArraySet(&exSensorEnabled, EX_POWER);
bitArraySet(&exSensorEnabled, EX_CAPACITY); bitArraySet(&exSensorEnabled, EX_CAPACITY);
} }
#ifdef USE_NAV
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
bitArraySet(&exSensorEnabled, EX_ALTITUDE); bitArraySet(&exSensorEnabled, EX_ALTITUDE);
bitArraySet(&exSensorEnabled, EX_VARIO); bitArraySet(&exSensorEnabled, EX_VARIO);
} }
#endif
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE); bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE); bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE);
@ -360,11 +358,9 @@ int32_t getSensorValue(uint8_t sensor)
return attitude.values.yaw; return attitude.values.yaw;
break; break;
#ifdef USE_NAV
case EX_VARIO: case EX_VARIO:
return getEstimatedActualVelocity(Z); return getEstimatedActualVelocity(Z);
break; break;
#endif
#ifdef USE_GPS #ifdef USE_GPS
case EX_GPS_SATS: case EX_GPS_SATS:

View file

@ -130,11 +130,7 @@ void ltm_gframe(sbuf_t *dst)
ltm_gs = gpsSol.groundSpeed / 100; ltm_gs = gpsSol.groundSpeed / 100;
} }
#if defined(USE_NAV)
ltm_alt = getEstimatedActualPosition(Z); // cm ltm_alt = getEstimatedActualPosition(Z); // cm
#else
ltm_alt = sensors(SENSOR_GPS) ? gpsSol.llh.alt : 0; // cm
#endif
sbufWriteU8(dst, 'G'); sbufWriteU8(dst, 'G');
sbufWriteU32(dst, ltm_lat); sbufWriteU32(dst, ltm_lat);
@ -251,7 +247,6 @@ void ltm_xframe(sbuf_t *dst)
ltm_x_counter++; // overflow is OK ltm_x_counter++; // overflow is OK
} }
#if defined(USE_NAV)
/** OSD additional data frame, ~4 Hz rate, navigation system status /** OSD additional data frame, ~4 Hz rate, navigation system status
*/ */
void ltm_nframe(sbuf_t *dst) void ltm_nframe(sbuf_t *dst)
@ -264,7 +259,6 @@ void ltm_nframe(sbuf_t *dst)
sbufWriteU8(dst, NAV_Status.error); sbufWriteU8(dst, NAV_Status.error);
sbufWriteU8(dst, NAV_Status.flags); sbufWriteU8(dst, NAV_Status.flags);
} }
#endif
#define LTM_BIT_AFRAME (1 << 0) #define LTM_BIT_AFRAME (1 << 0)
#define LTM_BIT_GFRAME (1 << 1) #define LTM_BIT_GFRAME (1 << 1)
@ -367,13 +361,11 @@ static void process_ltm(void)
ltm_finalise(dst); ltm_finalise(dst);
} }
#if defined(USE_NAV)
if (current_schedule & LTM_BIT_NFRAME) { if (current_schedule & LTM_BIT_NFRAME) {
ltm_initialise_packet(dst); ltm_initialise_packet(dst);
ltm_nframe(dst); ltm_nframe(dst);
ltm_finalise(dst); ltm_finalise(dst);
} }
#endif
ltm_scheduler = (ltm_scheduler + 1) % 10; ltm_scheduler = (ltm_scheduler + 1) % 10;
} }
@ -491,11 +483,9 @@ int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType)
case LTM_XFRAME: case LTM_XFRAME:
ltm_xframe(sbuf); ltm_xframe(sbuf);
break; break;
#if defined(USE_NAV)
case LTM_NFRAME: case LTM_NFRAME:
ltm_nframe(sbuf); ltm_nframe(sbuf);
break; break;
#endif
} }
sbufSwitchToReader(sbuf, ltmFrame); sbufSwitchToReader(sbuf, ltmFrame);
const int frameSize = sbufBytesRemaining(sbuf); const int frameSize = sbufBytesRemaining(sbuf);

View file

@ -28,9 +28,7 @@ typedef enum {
LTM_OFRAME, // Origin Frame LTM_OFRAME, // Origin Frame
LTM_XFRAME, // Extended information data frame LTM_XFRAME, // Extended information data frame
#endif #endif
#if defined(USE_NAV)
LTM_NFRAME, // Navigation Frame (inav extension) LTM_NFRAME, // Navigation Frame (inav extension)
#endif
LTM_FRAME_COUNT LTM_FRAME_COUNT
} ltm_frame_e; } ltm_frame_e;

View file

@ -580,11 +580,7 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
// alt Altitude in 1E3 meters (millimeters) above MSL // alt Altitude in 1E3 meters (millimeters) above MSL
gpsSol.llh.alt * 10, gpsSol.llh.alt * 10,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(USE_NAV)
getEstimatedActualPosition(Z) * 10, getEstimatedActualPosition(Z) * 10,
#else
gpsSol.llh.alt * 10,
#endif
// [cm/s] Ground X Speed (Latitude, positive north) // [cm/s] Ground X Speed (Latitude, positive north)
getEstimatedActualVelocity(X), getEstimatedActualVelocity(X),
// [cm/s] Ground Y Speed (Longitude, positive east) // [cm/s] Ground Y Speed (Longitude, positive east)
@ -654,16 +650,8 @@ void mavlinkSendHUDAndHeartbeat(void)
#endif #endif
// select best source for altitude // select best source for altitude
#if defined(USE_NAV)
mavAltitude = getEstimatedActualPosition(Z) / 100.0f; mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
#elif defined(USE_GPS)
if (sensors(SENSOR_GPS)) {
// No surface or baro, just display altitude above MLS
mavAltitude = gpsSol.llh.alt;
}
#endif
int16_t thr = rxGetChannelValue(THROTTLE); int16_t thr = rxGetChannelValue(THROTTLE);
if (navigationIsControllingThrottle()) { if (navigationIsControllingThrottle()) {

View file

@ -295,11 +295,7 @@ static void readSimResponse(void)
static int16_t getAltitudeMeters(void) static int16_t getAltitudeMeters(void)
{ {
#if defined(USE_NAV)
return getEstimatedActualPosition(Z) / 100; return getEstimatedActualPosition(Z) / 100;
#else
return sensors(SENSOR_GPS) ? gpsSol.llh.alt / 100 : 0;
#endif
} }
static void transmit(void) static void transmit(void)