mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Merge pull request #1229 from iNavFlight/nav-improvements
Navigation and GPS improvements
This commit is contained in:
Konstantin Sharlaimov
GitHub
commit
dcc163f185
8 changed files with 82 additions and 30 deletions
|
|
@ -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 |
|
||||
|
|
@ -158,6 +159,7 @@ Re-apply any new defaults as desired.
|
|||
| nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - right stick controls attitude like in ANGLE mode; CRUISE - right stick controls velocity in forward and right direction. |
|
||||
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
|
||||
| nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |
|
||||
| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value (cm) |
|
||||
| nav_max_speed | 300 | Maximum velocity firmware is allowed in full auto modes (POSHOLD, RTH, WP) [cm/s] [Multirotor only] |
|
||||
| nav_max_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s |
|
||||
| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
|
||||
|
|
|
|||
|
|
@ -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) },
|
||||
|
|
@ -792,6 +800,7 @@ static const clivalue_t valueTable[] = {
|
|||
{ "nav_user_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_NAV_USER_CTL_MODE }, PG_NAV_CONFIG, offsetof(navConfig_t, general.flags.user_control_mode) },
|
||||
{ "nav_position_timeout", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.pos_failure_timeout) },
|
||||
{ "nav_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 10000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.waypoint_radius) },
|
||||
{ "nav_wp_safe_distance", VAR_UINT16 | MASTER_VALUE | MODE_MAX, .config.max = { 65000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.waypoint_safe_distance) },
|
||||
{ "nav_max_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_speed) },
|
||||
{ "nav_max_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_climb_rate) },
|
||||
{ "nav_manual_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_manual_speed) },
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
// General navigation parameters
|
||||
.pos_failure_timeout = 5, // 5 sec
|
||||
.waypoint_radius = 100, // 2m diameter
|
||||
.waypoint_safe_distance = 10000, // 100m - first waypoint should be closer than this
|
||||
.max_speed = 300, // 3 m/s = 10.8 km/h
|
||||
.max_climb_rate = 500, // 5 m/s
|
||||
.max_manual_speed = 500,
|
||||
|
|
@ -2213,6 +2214,17 @@ void resetWaypointList(void)
|
|||
}
|
||||
}
|
||||
|
||||
static void mapWaypointToLocalPosition(t_fp_vector * localPos, const navWaypoint_t * waypoint)
|
||||
{
|
||||
gpsLocation_t wpLLH;
|
||||
|
||||
wpLLH.lat = waypoint->lat;
|
||||
wpLLH.lon = waypoint->lon;
|
||||
wpLLH.alt = waypoint->alt;
|
||||
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, localPos, GEO_ALT_RELATIVE);
|
||||
}
|
||||
|
||||
static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos)
|
||||
{
|
||||
posControl.activeWaypoint.pos = *pos;
|
||||
|
|
@ -2226,14 +2238,8 @@ static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos
|
|||
|
||||
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
||||
{
|
||||
gpsLocation_t wpLLH;
|
||||
t_fp_vector localPos;
|
||||
|
||||
wpLLH.lat = waypoint->lat;
|
||||
wpLLH.lon = waypoint->lon;
|
||||
wpLLH.alt = waypoint->alt;
|
||||
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &localPos, GEO_ALT_RELATIVE);
|
||||
mapWaypointToLocalPosition(&localPos, waypoint);
|
||||
calcualteAndSetActiveWaypointToLocalPosition(&localPos);
|
||||
}
|
||||
|
||||
|
|
@ -2333,7 +2339,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
|
||||
|
|
@ -2536,6 +2542,18 @@ bool naivationBlockArming(void)
|
|||
shouldBlockArming = true;
|
||||
}
|
||||
|
||||
// Don't allow arming if first waypoint is farther than configured safe distance
|
||||
if (posControl.waypointCount > 0) {
|
||||
t_fp_vector startingWaypointPos;
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
|
||||
|
||||
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
|
||||
|
||||
if (navWpMissionStartTooFar) {
|
||||
shouldBlockArming = true;
|
||||
}
|
||||
}
|
||||
|
||||
return shouldBlockArming;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -105,6 +111,7 @@ typedef struct navConfig_s {
|
|||
|
||||
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
|
||||
uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
|
||||
uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance
|
||||
uint16_t max_speed; // autonomous navigation speed cm/sec
|
||||
uint16_t max_climb_rate; // max vertical speed limitation cm/sec
|
||||
uint16_t max_manual_speed; // manual velocity control max horizontal speed
|
||||
|
|
|
|||
|
|
@ -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,20 @@ 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);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
#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 +320,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 +332,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 +429,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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue