mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 06:45:14 +03:00
Merge pull request #7617 from breadoven/abo_remove_NAV_define
Remove USE_NAV define
This commit is contained in:
commit
ac6650e25f
42 changed files with 26 additions and 389 deletions
|
@ -104,8 +104,8 @@ PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
|
||||||
.rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT,
|
.rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT,
|
||||||
.rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT,
|
.rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT,
|
||||||
.invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
|
.invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
|
||||||
.includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS |
|
.includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS |
|
||||||
BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE |
|
BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE |
|
||||||
BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | BLACKBOX_FEATURE_MOTORS,
|
BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | BLACKBOX_FEATURE_MOTORS,
|
||||||
);
|
);
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -855,7 +848,7 @@ static void writeIntraframe(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
|
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
|
||||||
|
|
||||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
|
||||||
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
|
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
|
||||||
}
|
}
|
||||||
|
@ -1111,7 +1104,7 @@ static void writeInterframe(void)
|
||||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
|
||||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
|
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
|
||||||
blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
|
blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
|
||||||
}
|
}
|
||||||
|
@ -1266,7 +1259,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_ESC_SENSOR
|
#ifdef USE_ESC_SENSOR
|
||||||
escSensorData_t * escSensor = escSensorGetData();
|
escSensorData_t * escSensor = escSensorGetData();
|
||||||
slow->escRPM = escSensor->rpm;
|
slow->escRPM = escSensor->rpm;
|
||||||
slow->escTemperature = escSensor->temperature;
|
slow->escTemperature = escSensor->temperature;
|
||||||
#endif
|
#endif
|
||||||
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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)) {
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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:
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
@ -84,7 +82,7 @@ static void processAirmodeMultirotor(void) {
|
||||||
*/
|
*/
|
||||||
DISABLE_STATE(AIRMODE_ACTIVE);
|
DISABLE_STATE(AIRMODE_ACTIVE);
|
||||||
} else if (
|
} else if (
|
||||||
!STATE(AIRMODE_ACTIVE) &&
|
!STATE(AIRMODE_ACTIVE) &&
|
||||||
rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold &&
|
rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold &&
|
||||||
(feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE))
|
(feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE))
|
||||||
) {
|
) {
|
||||||
|
@ -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)
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -739,7 +739,7 @@ static float dTermProcess(pidState_t *pidState, float dT) {
|
||||||
newDTerm = 0;
|
newDTerm = 0;
|
||||||
} else {
|
} else {
|
||||||
float delta = pidState->previousRateGyro - pidState->gyroRate;
|
float delta = pidState->previousRateGyro - pidState->gyroRate;
|
||||||
|
|
||||||
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
|
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
|
||||||
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
|
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
|
||||||
|
|
||||||
|
@ -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;
|
||||||
|
@ -1209,7 +1206,7 @@ void pidInit(void)
|
||||||
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||||
|
|
||||||
#ifdef USE_D_BOOST
|
#ifdef USE_D_BOOST
|
||||||
// Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
|
// Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
|
||||||
// We assume, max acceleration is when pilot deflects the stick fully in 100ms
|
// We assume, max acceleration is when pilot deflects the stick fully in 100ms
|
||||||
pidState[axis].dBoostTargetAcceleration = currentControlRateProfile->stabilized.rates[axis] * 10 * 10;
|
pidState[axis].dBoostTargetAcceleration = currentControlRateProfile->stabilized.rates[axis] * 10 * 10;
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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)) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
@ -216,7 +214,7 @@ void onNewGPSData(void)
|
||||||
if(STATE(GPS_FIX_HOME)){
|
if(STATE(GPS_FIX_HOME)){
|
||||||
static bool magDeclinationSet = false;
|
static bool magDeclinationSet = false;
|
||||||
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
|
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
|
||||||
const float declination = geoCalculateMagDeclination(&newLLH);
|
const float declination = geoCalculateMagDeclination(&newLLH);
|
||||||
imuSetMagneticDeclination(declination);
|
imuSetMagneticDeclination(declination);
|
||||||
#ifdef USE_SECONDARY_IMU
|
#ifdef USE_SECONDARY_IMU
|
||||||
secondaryImuSetMagneticDeclination(declination);
|
secondaryImuSetMagneticDeclination(declination);
|
||||||
|
@ -861,5 +859,3 @@ bool navIsCalibrationComplete(void)
|
||||||
{
|
{
|
||||||
return gravityCalibrationComplete();
|
return gravityCalibrationComplete();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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:
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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()) {
|
||||||
|
|
|
@ -280,7 +280,7 @@ static void readSimResponse(void)
|
||||||
simModuleState = SIM_MODULE_REGISTERED;
|
simModuleState = SIM_MODULE_REGISTERED;
|
||||||
} else {
|
} else {
|
||||||
simModuleState = SIM_MODULE_NOT_REGISTERED;
|
simModuleState = SIM_MODULE_NOT_REGISTERED;
|
||||||
}
|
}
|
||||||
} else if (responseCode == SIM_RESPONSE_CODE_CMT) {
|
} else if (responseCode == SIM_RESPONSE_CODE_CMT) {
|
||||||
// +CMT: <oa>,[<alpha>],<scts>[,<tooa>,<fo>,<pid>,<dcs>,<sca>,<tosca>,<length>]<CR><LF><data>
|
// +CMT: <oa>,[<alpha>],<scts>[,<tooa>,<fo>,<pid>,<dcs>,<sca>,<tosca>,<length>]<CR><LF><data>
|
||||||
// +CMT: "+3581234567","","19/02/12,14:57:24+08"
|
// +CMT: "+3581234567","","19/02/12,14:57:24+08"
|
||||||
|
@ -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)
|
||||||
|
@ -443,7 +439,7 @@ void handleSimTelemetry()
|
||||||
break;
|
break;
|
||||||
case SIM_STATE_INIT_ENTER_PIN:
|
case SIM_STATE_INIT_ENTER_PIN:
|
||||||
sendATCommand("AT+CPIN=");
|
sendATCommand("AT+CPIN=");
|
||||||
sendATCommand((char*)telemetryConfig()->simPin);
|
sendATCommand((char*)telemetryConfig()->simPin);
|
||||||
sendATCommand("\r");
|
sendATCommand("\r");
|
||||||
simTelemetryState = SIM_STATE_SET_MODES;
|
simTelemetryState = SIM_STATE_SET_MODES;
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue