Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Option to control altitude zero reset logic; Moved gps_num_sats to gpsConfig and made system-wide for GPS_FIX acquisition

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-02-04 22:03:09 +10:00
parent fa476e7f02
commit dd723fca2b
8 changed files with 52 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

3
docs/Cli.md
View file

@ -137,11 +137,12 @@ Re-apply any new defaults as desired.
| gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. |
| gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. |
| gps_auto_baud | ON | Automatic configuration of GPS baudrate(The spesified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports |
| gps_min_sats | 6 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. |
| inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. |
| inav_accz_unarmedcal | ON | Controls if inertial position estimator should compute gravity offset on accelerometer Z-axis dynamically when drone is unarmed. Mostly affects accuracy of altitude estimation and althold performace. No real reason to disable this feature. |
| inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. |
| inav_gps_delay | 200 | GPS position and velocity data usually arrive with a delay. This parameter defines this delay. Default (200) should be reasonable for most GPS receivers. |
| inav_gps_min_sats | 6 | Minimum number of GPS satellites in view to consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. |
| inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) |
| inav_w_z_baro_p | 0.350 | Weight of barometer measurements in estimated altitude and climb rate |
| inav_w_z_gps_p | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes |
| inav_w_z_gps_v | 0.500 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero |

10
src/main/fc/cli.c
View file

@ -290,6 +290,11 @@ static const char * const lookupTableNavControlMode[] = {
static const char * const lookupTableNavRthAltMode[] = {
"CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST"
};
static const char * const lookupTableNavResetAltitude[] = {
"NEVER", "FIRST_ARM", "EACH_ARM"
};
#endif
static const char * const lookupTableAuxOperator[] = {
@ -368,6 +373,7 @@ typedef enum {
#ifdef NAV
TABLE_NAV_USER_CTL_MODE,
TABLE_NAV_RTH_ALT_MODE,
TABLE_NAV_RESET_ALTITUDE,
#endif
TABLE_AUX_OPERATOR,
TABLE_MOTOR_PWM_PROTOCOL,
@ -422,6 +428,7 @@ static const lookupTableEntry_t lookupTables[] = {
#ifdef NAV
{ lookupTableNavControlMode, sizeof(lookupTableNavControlMode) / sizeof(char *) },
{ lookupTableNavRthAltMode, sizeof(lookupTableNavRthAltMode) / sizeof(char *) },
{ lookupTableNavResetAltitude, sizeof(lookupTableNavResetAltitude) / sizeof(char *) },
#endif
{ lookupTableAuxOperator, sizeof(lookupTableAuxOperator) / sizeof(char *) },
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
@ -678,6 +685,7 @@ static const clivalue_t valueTable[] = {
{ "gps_dyn_model", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_DYN_MODEL }, PG_GPS_CONFIG, offsetof(gpsConfig_t, dynModel) },
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoConfig) },
{ "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) },
{ "gps_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 5, 10}, PG_GPS_CONFIG, offsetof(gpsConfig_t, gpsMinSats) },
#endif
// PG_RC_CONTROLS_CONFIG
@ -771,7 +779,7 @@ static const clivalue_t valueTable[] = {
{ "inav_accz_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, accz_unarmed_cal) },
{ "inav_use_gps_velned", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, use_gps_velned) },
{ "inav_gps_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, gps_delay_ms) },
{ "inav_gps_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 5, 10}, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, gps_min_sats) },
{ "inav_reset_altitude", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_NAV_RESET_ALTITUDE }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, use_gps_velned) },
{ "inav_w_z_baro_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_baro_p) },
{ "inav_w_z_gps_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_p) },

4
src/main/fc/fc_msp.c
View file

@ -1208,7 +1208,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, positionEstimationConfig()->w_z_gps_v * 100); // 2 inav_w_z_gps_v float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_p * 100); // 2 inav_w_xy_gps_p float as value * 100
sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_v * 100); // 2 inav_w_xy_gps_v float as value * 100
sbufWriteU8(dst, positionEstimationConfig()->gps_min_sats); // 1 inav_gps_min_sats
sbufWriteU8(dst, 0); // 1 not used (was inav_gps_min_sats)
sbufWriteU8(dst, positionEstimationConfig()->use_gps_velned); // 1 inav_use_gps_velned ON/OFF
#else
@ -1680,7 +1680,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
positionEstimationConfigMutable()->w_z_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_xy_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->w_xy_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f);
positionEstimationConfigMutable()->gps_min_sats = constrain(sbufReadU8(src), 5, 10);
//positionEstimationConfigMutable()->gps_min_sats = constrain(sbufReadU8(src), 5, 10);
positionEstimationConfigMutable()->use_gps_velned = constrain(sbufReadU8(src), 0, 1);
#endif
break;

5
src/main/io/gps.c
View file

@ -120,7 +120,8 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.sbasMode = SBAS_NONE,
.autoConfig = GPS_AUTOCONFIG_ON,
.autoBaud = GPS_AUTOBAUD_ON,
.dynModel = GPS_DYNMODEL_AIR_1G
.dynModel = GPS_DYNMODEL_AIR_1G,
.gpsMinSats = 6
);
void gpsSetState(gpsState_e state)
@ -141,7 +142,7 @@ static void gpsHandleProtocol(void)
// Received new update for solution data
if (newDataReceived) {
// Set GPS fix flag only if we have 3D fix
if (gpsSol.fixType == GPS_FIX_3D) {
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
ENABLE_STATE(GPS_FIX);
}
else {

1
src/main/io/gps.h
View file

@ -85,6 +85,7 @@ typedef struct gpsConfig_s {
gpsAutoConfig_e autoConfig;
gpsAutoBaud_e autoBaud;
gpsDynModel_e dynModel;
uint8_t gpsMinSats;
} gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig);

2
src/main/navigation/navigation.c
View file

@ -2328,7 +2328,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
if ((STATE(GPS_FIX) && gpsSol.numSat >= positionEstimationConfig()->gps_min_sats)) navFlags |= (1 << 3);
//if (STATE(GPS_FIX)) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif

8
src/main/navigation/navigation.h
View file

@ -59,9 +59,15 @@ enum {
NAV_HEADING_CONTROL_MANUAL
};
enum {
NAV_RESET_ALTITUDE_NEVER = 0,
NAV_RESET_ALTITUDE_ON_FIRST_ARM,
NAV_RESET_ALTITUDE_ON_EACH_ARM,
};
typedef struct positionEstimationConfig_s {
uint8_t automatic_mag_declination;
uint8_t gps_min_sats;
uint8_t reset_altitude_type;
uint8_t accz_unarmed_cal;
uint8_t use_gps_velned;
uint16_t gps_delay_ms;

28
src/main/navigation/navigation_pos_estimator.c
View file

@ -171,7 +171,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationCo
PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters
.automatic_mag_declination = 1,
.gps_min_sats = 6,
.reset_altitude_type = NAV_RESET_ALTITUDE_ON_FIRST_ARM,
.gps_delay_ms = 200,
.accz_unarmed_cal = 1,
.use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
@ -226,6 +226,18 @@ static bool updateTimer(navigationTimer_t * tim, uint32_t interval, timeUs_t cur
}
}
static bool shouldResetReferenceAltitude(void)
{
switch (positionEstimationConfig()->reset_altitude_type) {
case NAV_RESET_ALTITUDE_NEVER:
return false;
case NAV_RESET_ALTITUDE_ON_FIRST_ARM:
return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED);
case NAV_RESET_ALTITUDE_ON_EACH_ARM:
return !ARMING_FLAG(ARMED);
}
}
#if defined(GPS)
/* Why is this here: Because GPS will be sending at quiet a nailed rate (if not overloaded by junk tasks at the brink of its specs)
* but we might read out with timejitter because Irq might be off by a few us so we do a +-10% margin around the time between GPS
@ -306,7 +318,7 @@ void onNewGPSData(void)
newLLH.alt = gpsSol.llh.alt;
if (sensors(SENSOR_GPS)) {
if (!(STATE(GPS_FIX) && gpsSol.numSat >= positionEstimationConfig()->gps_min_sats)) {
if (!STATE(GPS_FIX)) {
isFirstGPSUpdate = true;
return;
}
@ -318,24 +330,24 @@ void onNewGPSData(void)
#if defined(NAV_AUTO_MAG_DECLINATION)
/* Automatic magnetic declination calculation - do this once */
static bool magDeclinationSet = false;
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet && (gpsSol.numSat >= positionEstimationConfig()->gps_min_sats)) {
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
mag.magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units
magDeclinationSet = true;
}
#endif
/* Process position update if GPS origin is already set, or precision is good enough */
// FIXME: use HDOP here
if ((posControl.gpsOrigin.valid) || (gpsSol.numSat >= positionEstimationConfig()->gps_min_sats)) {
// FIXME: Add HDOP check for acquisition of GPS origin
/* Set GPS origin or reset the origin altitude - keep initial pre-arming altitude at zero */
if (!posControl.gpsOrigin.valid) {
geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET);
}
else if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED)) {
else if (shouldResetReferenceAltitude()) {
/* If we were never armed - keep altitude at zero */
geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE);
}
if (posControl.gpsOrigin.valid) {
/* Convert LLH position to local coordinates */
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, & posEstimator.gps.pos, GEO_ALT_ABSOLUTE);
@ -415,8 +427,8 @@ static void updateBaroTopic(timeUs_t currentTimeUs)
static float initialBaroAltitudeOffset = 0.0f;
float newBaroAlt = baroCalculateAltitude();
/* If we were never armed - keep altitude at zero */
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED)) {
/* If we are required - keep altitude at zero */
if (shouldResetReferenceAltitude()) {
initialBaroAltitudeOffset = newBaroAlt;
}