mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 12:25:20 +03:00
remove gps_required and gps_minimum sats and revert to gps_rescue_min_sats
This commit is contained in:
parent
c5468981e6
commit
341d65becf
22 changed files with 41 additions and 63 deletions
|
@ -1525,8 +1525,6 @@ static bool blackboxWriteSysinfo(void)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_REQUIRED_SATS, "%d", gpsConfig()->gpsRequiredSats)
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_MINIMUM_SATS, "%d", gpsConfig()->gpsMinimumSats)
|
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth)
|
||||||
|
@ -1548,6 +1546,7 @@ static bool blackboxWriteSysinfo(void)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover)
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
|
||||||
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats)
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
|
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
|
||||||
|
|
|
@ -1009,8 +1009,6 @@ const clivalue_t valueTable[] = {
|
||||||
{ PARAM_NAME_GPS_UBLOX_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_mode) },
|
{ PARAM_NAME_GPS_UBLOX_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_mode) },
|
||||||
{ PARAM_NAME_GPS_SET_HOME_POINT_ONCE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) },
|
{ PARAM_NAME_GPS_SET_HOME_POINT_ONCE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) },
|
||||||
{ PARAM_NAME_GPS_USE_3D_SPEED, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_use_3d_speed) },
|
{ PARAM_NAME_GPS_USE_3D_SPEED, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_use_3d_speed) },
|
||||||
{ PARAM_NAME_GPS_REQUIRED_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {5, 50}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gpsRequiredSats) },
|
|
||||||
{ PARAM_NAME_GPS_MINIMUM_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {4, 50}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gpsMinimumSats) },
|
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
// PG_GPS_RESCUE
|
// PG_GPS_RESCUE
|
||||||
|
@ -1033,6 +1031,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) },
|
{ PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) },
|
||||||
|
|
||||||
{ PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
|
{ PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
|
||||||
|
{ PARAM_NAME_GPS_RESCUE_MIN_SATS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
|
||||||
{ PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
|
{ PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
|
||||||
|
|
||||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
|
{ PARAM_NAME_GPS_RESCUE_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
|
||||||
|
|
|
@ -38,7 +38,6 @@
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
|
||||||
#include "flight/gps_rescue.h"
|
#include "flight/gps_rescue.h"
|
||||||
#include "io/gps.h"
|
|
||||||
|
|
||||||
static uint16_t gpsRescueConfig_minRescueDth; //meters
|
static uint16_t gpsRescueConfig_minRescueDth; //meters
|
||||||
static uint8_t gpsRescueConfig_altitudeMode;
|
static uint8_t gpsRescueConfig_altitudeMode;
|
||||||
|
@ -57,7 +56,7 @@ static uint16_t gpsRescueConfig_throttleMin;
|
||||||
static uint16_t gpsRescueConfig_throttleMax;
|
static uint16_t gpsRescueConfig_throttleMax;
|
||||||
static uint16_t gpsRescueConfig_throttleHover;
|
static uint16_t gpsRescueConfig_throttleHover;
|
||||||
|
|
||||||
static uint8_t gpsConfig_gpsRequiredSats;
|
static uint8_t gpsRescueConfig_minSats;
|
||||||
static uint8_t gpsRescueConfig_allowArmingWithoutFix;
|
static uint8_t gpsRescueConfig_allowArmingWithoutFix;
|
||||||
|
|
||||||
static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
|
static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
|
||||||
|
@ -150,7 +149,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
|
||||||
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
|
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
|
||||||
gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover;
|
gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover;
|
||||||
|
|
||||||
gpsConfig_gpsRequiredSats = gpsConfig()->gpsRequiredSats;
|
gpsRescueConfig_minSats = gpsRescueConfig()->minSats;
|
||||||
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
|
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
|
@ -178,7 +177,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
|
||||||
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
|
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
|
||||||
gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover;
|
gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover;
|
||||||
|
|
||||||
gpsConfigMutable()->gpsRequiredSats = gpsConfig_gpsRequiredSats;
|
gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats;
|
||||||
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
|
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
|
@ -205,7 +204,7 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
|
||||||
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } },
|
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } },
|
||||||
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } },
|
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } },
|
||||||
|
|
||||||
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsConfig_gpsRequiredSats, 4, 50, 1 } },
|
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } },
|
||||||
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },
|
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },
|
||||||
|
|
||||||
{ "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},
|
{ "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},
|
||||||
|
|
|
@ -209,14 +209,6 @@ static void validateAndFixRatesSettings(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void validateAndFixMinSatsGpsConfig(void)
|
|
||||||
{
|
|
||||||
if (gpsConfig()->gpsMinimumSats >= gpsConfig()->gpsRequiredSats) {
|
|
||||||
gpsConfigMutable()->gpsRequiredSats = GPS_REQUIRED_SAT_COUNT;
|
|
||||||
gpsConfigMutable()->gpsMinimumSats = GPS_MINIMUM_SAT_COUNT;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void validateAndFixConfig(void)
|
static void validateAndFixConfig(void)
|
||||||
{
|
{
|
||||||
#if !defined(USE_QUAD_MIXER_ONLY)
|
#if !defined(USE_QUAD_MIXER_ONLY)
|
||||||
|
@ -602,8 +594,6 @@ static void validateAndFixConfig(void)
|
||||||
// This should be done at the end of the validation
|
// This should be done at the end of the validation
|
||||||
targetValidateConfiguration();
|
targetValidateConfiguration();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
validateAndFixMinSatsGpsConfig();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void validateAndFixGyroConfig(void)
|
void validateAndFixGyroConfig(void)
|
||||||
|
|
|
@ -345,7 +345,7 @@ void updateArmingStatus(void)
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
if (gpsRescueIsConfigured()) {
|
if (gpsRescueIsConfigured()) {
|
||||||
if (gpsRescueConfig()->allowArmingWithoutFix || (STATE(GPS_FIX) && (gpsSol.numSat >= gpsConfig()->gpsRequiredSats)) ||
|
if (gpsRescueConfig()->allowArmingWithoutFix || (STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) ||
|
||||||
ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
|
ARMING_FLAG(WAS_EVER_ARMED) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
|
||||||
unsetArmingDisabled(ARMING_DISABLED_GPS);
|
unsetArmingDisabled(ARMING_DISABLED_GPS);
|
||||||
} else {
|
} else {
|
||||||
|
@ -577,7 +577,7 @@ void tryArm(void)
|
||||||
GPS_reset_home_position();
|
GPS_reset_home_position();
|
||||||
//beep to indicate arming
|
//beep to indicate arming
|
||||||
if (featureIsEnabled(FEATURE_GPS)) {
|
if (featureIsEnabled(FEATURE_GPS)) {
|
||||||
if (STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) {
|
if (STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
|
||||||
beeper(BEEPER_ARMING_GPS_FIX);
|
beeper(BEEPER_ARMING_GPS_FIX);
|
||||||
} else {
|
} else {
|
||||||
beeper(BEEPER_ARMING_GPS_NO_FIX);
|
beeper(BEEPER_ARMING_GPS_NO_FIX);
|
||||||
|
|
|
@ -133,8 +133,6 @@
|
||||||
#define PARAM_NAME_GPS_UBLOX_MODE "gps_ublox_mode"
|
#define PARAM_NAME_GPS_UBLOX_MODE "gps_ublox_mode"
|
||||||
#define PARAM_NAME_GPS_SET_HOME_POINT_ONCE "gps_set_home_point_once"
|
#define PARAM_NAME_GPS_SET_HOME_POINT_ONCE "gps_set_home_point_once"
|
||||||
#define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed"
|
#define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed"
|
||||||
#define PARAM_NAME_GPS_REQUIRED_SATS "gps_required_sats"
|
|
||||||
#define PARAM_NAME_GPS_MINIMUM_SATS "gps_minimum_sats"
|
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
#define PARAM_NAME_GPS_RESCUE_MIN_START_DIST "gps_rescue_min_start_dist"
|
#define PARAM_NAME_GPS_RESCUE_MIN_START_DIST "gps_rescue_min_start_dist"
|
||||||
|
@ -156,6 +154,7 @@
|
||||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER "gps_rescue_throttle_hover"
|
#define PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER "gps_rescue_throttle_hover"
|
||||||
|
|
||||||
#define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks"
|
#define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks"
|
||||||
|
#define PARAM_NAME_GPS_RESCUE_MIN_SATS "gps_rescue_min_sats"
|
||||||
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
|
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
|
||||||
|
|
||||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p"
|
#define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p"
|
||||||
|
|
|
@ -167,6 +167,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||||
|
|
||||||
.allowArmingWithoutFix = false,
|
.allowArmingWithoutFix = false,
|
||||||
.sanityChecks = RESCUE_SANITY_FS_ONLY,
|
.sanityChecks = RESCUE_SANITY_FS_ONLY,
|
||||||
|
.minSats = 8,
|
||||||
|
|
||||||
.throttleP = 15,
|
.throttleP = 15,
|
||||||
.throttleI = 15,
|
.throttleI = 15,
|
||||||
|
@ -507,7 +508,7 @@ static void performSanityChecks(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
secondsLowSats += (!STATE(GPS_FIX) || (gpsSol.numSat < gpsConfig()->gpsMinimumSats)) ? 1 : -1;
|
secondsLowSats += (!STATE(GPS_FIX) || (gpsSol.numSat < GPS_MIN_SAT_COUNT)) ? 1 : -1;
|
||||||
secondsLowSats = constrain(secondsLowSats, 0, 10);
|
secondsLowSats = constrain(secondsLowSats, 0, 10);
|
||||||
|
|
||||||
if (secondsLowSats == 10) {
|
if (secondsLowSats == 10) {
|
||||||
|
@ -654,7 +655,7 @@ static bool checkGPSRescueIsAvailable(void)
|
||||||
noGPSfix = false;
|
noGPSfix = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < gpsConfig()->gpsMinimumSats) ? 1 : -1), 0, 2);
|
secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < GPS_MIN_SAT_COUNT) ? 1 : -1), 0, 2);
|
||||||
if (secondsLowSats == 2) {
|
if (secondsLowSats == 2) {
|
||||||
lowsats = true;
|
lowsats = true;
|
||||||
result = false;
|
result = false;
|
||||||
|
|
|
@ -31,6 +31,7 @@ typedef struct gpsRescue_s {
|
||||||
uint16_t throttleMin;
|
uint16_t throttleMin;
|
||||||
uint16_t throttleMax;
|
uint16_t throttleMax;
|
||||||
uint16_t throttleHover;
|
uint16_t throttleHover;
|
||||||
|
uint8_t minSats;
|
||||||
uint8_t velP, velI, velD;
|
uint8_t velP, velI, velD;
|
||||||
uint16_t minRescueDth; //meters
|
uint16_t minRescueDth; //meters
|
||||||
uint8_t sanityChecks;
|
uint8_t sanityChecks;
|
||||||
|
|
|
@ -490,7 +490,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
#if defined(USE_GPS)
|
#if defined(USE_GPS)
|
||||||
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > gpsConfig()->gpsMinimumSats && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
|
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
|
||||||
// Use GPS course over ground to correct attitude.values.yaw
|
// Use GPS course over ground to correct attitude.values.yaw
|
||||||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||||
useCOG = true;
|
useCOG = true;
|
||||||
|
|
|
@ -359,7 +359,7 @@ static void beeperGpsStatus(void)
|
||||||
{
|
{
|
||||||
if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) {
|
if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(BEEPER_GPS_STATUS))) {
|
||||||
// if GPS 3D fix and at least the minimum number available, then beep out number of satellites
|
// if GPS 3D fix and at least the minimum number available, then beep out number of satellites
|
||||||
if (STATE(GPS_FIX) && gpsSol.numSat > gpsConfig()->gpsMinimumSats) {
|
if (STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT) {
|
||||||
uint8_t i = 0;
|
uint8_t i = 0;
|
||||||
do {
|
do {
|
||||||
beep_multiBeeps[i++] = 5;
|
beep_multiBeeps[i++] = 5;
|
||||||
|
|
|
@ -273,7 +273,7 @@ typedef enum {
|
||||||
|
|
||||||
gpsData_t gpsData;
|
gpsData_t gpsData;
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 1);
|
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
|
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
|
||||||
.provider = GPS_UBLOX,
|
.provider = GPS_UBLOX,
|
||||||
|
@ -285,8 +285,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
|
||||||
.gps_set_home_point_once = false,
|
.gps_set_home_point_once = false,
|
||||||
.gps_use_3d_speed = false,
|
.gps_use_3d_speed = false,
|
||||||
.sbas_integrity = false,
|
.sbas_integrity = false,
|
||||||
.gpsRequiredSats = GPS_REQUIRED_SAT_COUNT,
|
|
||||||
.gpsMinimumSats = GPS_MINIMUM_SAT_COUNT
|
|
||||||
);
|
);
|
||||||
|
|
||||||
static void shiftPacketLog(void)
|
static void shiftPacketLog(void)
|
||||||
|
@ -764,7 +762,7 @@ void gpsInitHardware(void)
|
||||||
static void updateGpsIndicator(timeUs_t currentTimeUs)
|
static void updateGpsIndicator(timeUs_t currentTimeUs)
|
||||||
{
|
{
|
||||||
static uint32_t GPSLEDTime;
|
static uint32_t GPSLEDTime;
|
||||||
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsConfig()->gpsRequiredSats)) {
|
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && STATE(GPS_FIX) && (gpsSol.numSat >= gpsRescueConfig()->minSats)) {
|
||||||
GPSLEDTime = currentTimeUs + 150000;
|
GPSLEDTime = currentTimeUs + 150000;
|
||||||
LED1_TOGGLE;
|
LED1_TOGGLE;
|
||||||
}
|
}
|
||||||
|
@ -886,7 +884,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
|
||||||
if (!ARMING_FLAG(ARMED)) {
|
if (!ARMING_FLAG(ARMED)) {
|
||||||
// while disarmed, beep when requirements for a home fix are met
|
// while disarmed, beep when requirements for a home fix are met
|
||||||
// ?? should we also beep if home fix requirements first appear after arming?
|
// ?? should we also beep if home fix requirements first appear after arming?
|
||||||
if (!hasBeeped && STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) {
|
if (!hasBeeped && STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
|
||||||
beeper(BEEPER_READY_BEEP);
|
beeper(BEEPER_READY_BEEP);
|
||||||
hasBeeped = true;
|
hasBeeped = true;
|
||||||
}
|
}
|
||||||
|
@ -1810,7 +1808,7 @@ void GPS_reset_home_position(void)
|
||||||
// runs, if GPS is defined, on arming via tryArm() in core.c, and on gyro cal via processRcStickPositions() in rc_controls.c
|
// runs, if GPS is defined, on arming via tryArm() in core.c, and on gyro cal via processRcStickPositions() in rc_controls.c
|
||||||
{
|
{
|
||||||
if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
|
if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
|
||||||
if (STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsRequiredSats) {
|
if (STATE(GPS_FIX) && gpsSol.numSat >= gpsRescueConfig()->minSats) {
|
||||||
// those checks are always true for tryArm, but may not be true for gyro cal
|
// those checks are always true for tryArm, but may not be true for gyro cal
|
||||||
GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
|
GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
|
||||||
GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
|
GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
|
||||||
|
@ -1860,7 +1858,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
|
||||||
|
|
||||||
void onGpsNewData(void)
|
void onGpsNewData(void)
|
||||||
{
|
{
|
||||||
if (!(STATE(GPS_FIX) && gpsSol.numSat >= gpsConfig()->gpsMinimumSats)) {
|
if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
|
||||||
// if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue
|
// if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
#define GPS_DEGREES_DIVIDER 10000000L
|
#define GPS_DEGREES_DIVIDER 10000000L
|
||||||
#define GPS_X 1
|
#define GPS_X 1
|
||||||
#define GPS_Y 0
|
#define GPS_Y 0
|
||||||
|
#define GPS_MIN_SAT_COUNT 4 // number of sats to trigger low sat count sanity check
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
GPS_LATITUDE,
|
GPS_LATITUDE,
|
||||||
|
@ -82,8 +83,6 @@ typedef enum {
|
||||||
UBLOX_ACK_GOT_NACK
|
UBLOX_ACK_GOT_NACK
|
||||||
} ubloxAckState_e;
|
} ubloxAckState_e;
|
||||||
|
|
||||||
#define GPS_REQUIRED_SAT_COUNT 8
|
|
||||||
#define GPS_MINIMUM_SAT_COUNT 4
|
|
||||||
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
|
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
|
||||||
|
|
||||||
typedef struct gpsConfig_s {
|
typedef struct gpsConfig_s {
|
||||||
|
@ -96,8 +95,6 @@ typedef struct gpsConfig_s {
|
||||||
uint8_t gps_set_home_point_once;
|
uint8_t gps_set_home_point_once;
|
||||||
uint8_t gps_use_3d_speed;
|
uint8_t gps_use_3d_speed;
|
||||||
uint8_t sbas_integrity;
|
uint8_t sbas_integrity;
|
||||||
uint8_t gpsRequiredSats;
|
|
||||||
uint8_t gpsMinimumSats;
|
|
||||||
} gpsConfig_t;
|
} gpsConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(gpsConfig_t, gpsConfig);
|
PG_DECLARE(gpsConfig_t, gpsConfig);
|
||||||
|
|
|
@ -1489,9 +1489,6 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
|
||||||
// Added in API version 1.43
|
// Added in API version 1.43
|
||||||
sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once);
|
sbufWriteU8(dst, gpsConfig()->gps_set_home_point_once);
|
||||||
sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo);
|
sbufWriteU8(dst, gpsConfig()->gps_ublox_use_galileo);
|
||||||
// Added in API version 1.45
|
|
||||||
sbufWriteU8(dst, gpsConfig()->gpsRequiredSats);
|
|
||||||
sbufWriteU8(dst, gpsConfig()->gpsMinimumSats);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_RAW_GPS:
|
case MSP_RAW_GPS:
|
||||||
|
@ -1532,7 +1529,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
|
||||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
|
sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
|
||||||
sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
|
sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
|
||||||
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
|
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
|
||||||
sbufWriteU8(dst, 0); // not required in API 1.44, gpsRescueConfig()->minSats
|
sbufWriteU8(dst, gpsRescueConfig()->minSats);
|
||||||
|
|
||||||
// Added in API version 1.43
|
// Added in API version 1.43
|
||||||
sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
|
sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
|
||||||
|
@ -2740,12 +2737,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
||||||
gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src);
|
gpsConfigMutable()->gps_set_home_point_once = sbufReadU8(src);
|
||||||
gpsConfigMutable()->gps_ublox_use_galileo = sbufReadU8(src);
|
gpsConfigMutable()->gps_ublox_use_galileo = sbufReadU8(src);
|
||||||
}
|
}
|
||||||
if (sbufBytesRemaining(src) >= 2) {
|
|
||||||
// Added in API version 1.45
|
|
||||||
gpsConfigMutable()->gpsRequiredSats = sbufReadU8(src);
|
|
||||||
gpsConfigMutable()->gpsMinimumSats = sbufReadU8(src);
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
|
@ -2758,7 +2749,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
||||||
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
|
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
|
||||||
gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
|
gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
|
||||||
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
|
gpsRescueConfigMutable()->sanityChecks = sbufReadU8(src);
|
||||||
sbufReadU8(src); // not used since 1.43, was gps rescue minSats
|
gpsRescueConfigMutable()->minSats = sbufReadU8(src);
|
||||||
if (sbufBytesRemaining(src) >= 6) {
|
if (sbufBytesRemaining(src) >= 6) {
|
||||||
// Added in API version 1.43
|
// Added in API version 1.43
|
||||||
gpsRescueConfigMutable()->ascendRate = sbufReadU16(src);
|
gpsRescueConfigMutable()->ascendRate = sbufReadU16(src);
|
||||||
|
|
|
@ -315,7 +315,7 @@
|
||||||
#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
|
#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
|
||||||
#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings
|
#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings
|
||||||
#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
|
#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
|
||||||
#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration including from 1.44 minSats
|
#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
|
||||||
//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
|
//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
|
||||||
#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed and sanityChecks
|
#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed and sanityChecks
|
||||||
#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P
|
#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P
|
||||||
|
|
|
@ -1953,9 +1953,9 @@ void osdUpdateAlarms(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_GPS
|
#ifdef USE_GPS
|
||||||
if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < gpsConfig()->gpsMinimumSats)
|
if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < GPS_MIN_SAT_COUNT)
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
|| ((gpsSol.numSat < gpsConfig()->gpsRequiredSats) && gpsRescueIsConfigured())
|
|| ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
|
||||||
#endif
|
#endif
|
||||||
) {
|
) {
|
||||||
SET_BLINK(OSD_GPS_SATS);
|
SET_BLINK(OSD_GPS_SATS);
|
||||||
|
|
|
@ -218,7 +218,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (gpsSol.numSat >= gpsConfig()->gpsMinimumSats) {
|
if (gpsSol.numSat >= GPS_MIN_SAT_COUNT) {
|
||||||
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
|
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
|
||||||
} else {
|
} else {
|
||||||
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
|
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
|
||||||
|
|
|
@ -307,9 +307,8 @@ static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value)
|
||||||
|
|
||||||
uint16_t gpsFixType = 0;
|
uint16_t gpsFixType = 0;
|
||||||
uint16_t sats = 0;
|
uint16_t sats = 0;
|
||||||
uint8_t minSats = gpsConfig()->gpsMinimumSats;
|
|
||||||
if (sensors(SENSOR_GPS)) {
|
if (sensors(SENSOR_GPS)) {
|
||||||
gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < minSats ? 2 : 3);
|
gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < GPS_MIN_SAT_COUNT ? 2 : 3);
|
||||||
sats = gpsSol.numSat;
|
sats = gpsSol.numSat;
|
||||||
if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) {
|
if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) {
|
||||||
result = true;
|
result = true;
|
||||||
|
|
|
@ -137,7 +137,7 @@ static void ltm_gframe(void)
|
||||||
|
|
||||||
if (!STATE(GPS_FIX))
|
if (!STATE(GPS_FIX))
|
||||||
gps_fix_type = 1;
|
gps_fix_type = 1;
|
||||||
else if (gpsSol.numSat < gpsConfig()->gpsMinimumSats)
|
else if (gpsSol.numSat < GPS_MIN_SAT_COUNT)
|
||||||
gps_fix_type = 2;
|
gps_fix_type = 2;
|
||||||
else
|
else
|
||||||
gps_fix_type = 3;
|
gps_fix_type = 3;
|
||||||
|
|
|
@ -310,7 +310,7 @@ void mavlinkSendPosition(void)
|
||||||
gpsFixType = 1;
|
gpsFixType = 1;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
if (gpsSol.numSat < gpsConfig()->gpsMinimumSats) {
|
if (gpsSol.numSat < GPS_MIN_SAT_COUNT) {
|
||||||
gpsFixType = 2;
|
gpsFixType = 2;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
|
|
@ -296,7 +296,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
|
||||||
uint16_t altitudeLoBcd, groundCourseBcd, hdop;
|
uint16_t altitudeLoBcd, groundCourseBcd, hdop;
|
||||||
uint8_t hdopBcd, gpsFlags;
|
uint8_t hdopBcd, gpsFlags;
|
||||||
|
|
||||||
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < gpsConfig()->gpsMinimumSats) {
|
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < GPS_MIN_SAT_COUNT) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -364,7 +364,7 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
|
||||||
uint8_t numSatBcd, altitudeHighBcd;
|
uint8_t numSatBcd, altitudeHighBcd;
|
||||||
bool timeProvided = false;
|
bool timeProvided = false;
|
||||||
|
|
||||||
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < gpsConfig()->gpsMinimumSats) {
|
if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < GPS_MIN_SAT_COUNT) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1193,8 +1193,6 @@ TEST_F(OsdTest, TestGpsElements)
|
||||||
{
|
{
|
||||||
// given
|
// given
|
||||||
osdElementConfigMutable()->item_pos[OSD_GPS_SATS] = OSD_POS(2, 4) | OSD_PROFILE_1_FLAG;
|
osdElementConfigMutable()->item_pos[OSD_GPS_SATS] = OSD_POS(2, 4) | OSD_PROFILE_1_FLAG;
|
||||||
gpsConfigMutable()->gpsMinimumSats = GPS_MINIMUM_SAT_COUNT;
|
|
||||||
gpsConfigMutable()->gpsRequiredSats = GPS_REQUIRED_SAT_COUNT;
|
|
||||||
|
|
||||||
sensorsSet(SENSOR_GPS);
|
sensorsSet(SENSOR_GPS);
|
||||||
osdAnalyzeActiveElements();
|
osdAnalyzeActiveElements();
|
||||||
|
|
|
@ -22,26 +22,33 @@ extern "C" {
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/streambuf.h"
|
#include "common/streambuf.h"
|
||||||
|
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
|
||||||
#include "fc/controlrate_profile.h"
|
#include "fc/controlrate_profile.h"
|
||||||
#include "fc/core.h"
|
#include "fc/core.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/rc_modes.h"
|
#include "fc/rc_modes.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/servos.h"
|
#include "flight/servos.h"
|
||||||
|
#include "flight/gps_rescue.h"
|
||||||
|
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
#include "io/gps.h"
|
#include "io/gps.h"
|
||||||
#include "io/vtx.h"
|
#include "io/vtx.h"
|
||||||
|
|
||||||
#include "pg/motor.h"
|
#include "pg/motor.h"
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/pg_ids.h"
|
||||||
#include "pg/rx.h"
|
#include "pg/rx.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
#include "sensors/acceleration.h"
|
#include "sensors/acceleration.h"
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
|
@ -61,7 +68,7 @@ extern "C" {
|
||||||
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
|
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
|
||||||
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
|
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
|
||||||
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
|
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
|
||||||
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_CONFIG, 0);
|
||||||
|
|
||||||
float rcCommand[4];
|
float rcCommand[4];
|
||||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue