Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Remove dead reckoning possibility. Removed throttle_tilt_compensation option. Added extra arming safety - disable arming if no GPS fix

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-27 15:25:41 +10:00
parent 62aaa50f93
commit d731486235
7 changed files with 24 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/config/config.c
View file

@ -214,7 +214,7 @@ void resetNavConfig(navConfig_t * navConfig)
{
// Navigation flags
navConfig->flags.use_midrc_for_althold = 0;
navConfig->flags.throttle_tilt_comp = 1;
navConfig->flags.extra_arming_safety = 1;
navConfig->flags.user_control_mode = NAV_GPS_ATTI;
navConfig->flags.rth_alt_control_style = NAV_RTH_AT_LEAST_ALT;
@ -222,7 +222,6 @@ void resetNavConfig(navConfig_t * navConfig)
#if defined(INAV_ENABLE_AUTO_MAG_DECLINATION)
navConfig->inav.automatic_mag_declination = 1;
#endif
navConfig->inav.enable_dead_reckoning = 0;
navConfig->inav.gps_delay_ms = 200;
navConfig->inav.accz_unarmed_cal = 1;

8
src/main/flight/navigation_rewrite.c
View file

@ -1704,7 +1704,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
*-----------------------------------------------------------*/
bool navigationRequiresThrottleTiltCompensation(void)
{
return !STATE(FIXED_WING) && posControl.navConfig->flags.throttle_tilt_comp && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
return !STATE(FIXED_WING) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
}
/*-----------------------------------------------------------
@ -1733,6 +1733,12 @@ int8_t naivationGetHeadingControlState(void)
}
}
bool naivationBlockArming(void)
{
bool okToArm = posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME);
return (posControl.navConfig->flags.extra_arming_safety && !okToArm);
}
/**
* Indicate ready/not ready status
*/

4
src/main/flight/navigation_rewrite.h
View file

@ -60,7 +60,7 @@ enum {
typedef struct navConfig_s {
struct {
uint8_t use_midrc_for_althold; // Don't remember throttle when althold was initiated, assume that throttle is at middle = zero climb rate
uint8_t throttle_tilt_comp; // Forcibly apply 100% throttle tilt compensation
uint8_t extra_arming_safety; // Forcibly apply 100% throttle tilt compensation
uint8_t user_control_mode; // NAV_GPS_ATTI or NAV_GPS_CRUISE
uint8_t rth_alt_control_style; // Controls how RTH controls altitude
} flags;
@ -69,7 +69,6 @@ typedef struct navConfig_s {
#if defined(INAV_ENABLE_AUTO_MAG_DECLINATION)
uint8_t automatic_mag_declination;
#endif
uint8_t enable_dead_reckoning;
uint8_t accz_unarmed_cal;
uint16_t gps_delay_ms;
@ -163,6 +162,7 @@ void updateHomePosition(void);
bool naivationRequiresAngleMode(void);
bool navigationRequiresThrottleTiltCompensation(void);
int8_t naivationGetHeadingControlState(void);
bool naivationBlockArming(void);
float getEstimatedActualVelocity(int axis);
float getEstimatedActualPosition(int axis);

12
src/main/flight/navigation_rewrite_pos_estimator.c
View file

@ -467,7 +467,6 @@ static void updateEstimatedTopic(uint32_t currentTime)
sensors(SENSOR_GPS) && ((currentTime - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS));
bool isBaroValid = sensors(SENSOR_BARO) && ((currentTime - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
bool isSonarValid = sensors(SENSOR_SONAR) && ((currentTime - posEstimator.sonar.lastUpdateTime) <= MS2US(INAV_SONAR_TIMEOUT_MS));
bool useDeadReckoning = posControl.navConfig->inav.enable_dead_reckoning && (!isGPSValid);
#if defined(INAV_ENABLE_GPS_GLITCH_DETECTION)
//isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
@ -545,7 +544,7 @@ static void updateEstimatedTopic(uint32_t currentTime)
}
/* Estimate XY-axis */
if ((posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) || isGPSValid || useDeadReckoning) {
if ((posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) || isGPSValid) {
/* Predict position */
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X);
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y);
@ -561,15 +560,6 @@ static void updateEstimatedTopic(uint32_t currentTime)
/* Adjust EPH */
posEstimator.est.eph = MIN(posEstimator.est.eph, posEstimator.gps.eph);
}
/* Use dead reckoning */
if (useDeadReckoning) {
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, posControl.navConfig->inav.w_xy_dr_v);
inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.V.Y, posControl.navConfig->inav.w_xy_dr_v);
/* Adjust EPH to just below max_epe */
posEstimator.est.eph = posControl.navConfig->inav.max_eph_epv / (1.0f + dt);
}
}
else {
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, posControl.navConfig->inav.w_xy_res_v);

3
src/main/io/serial_cli.c
View file

@ -537,7 +537,6 @@ const clivalue_t valueTable[] = {
#endif
{ "inav_accz_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.inav.accz_unarmed_cal, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "inav_dead_reckoning", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.inav.enable_dead_reckoning, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "inav_gps_delay", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.inav.gps_delay_ms, .config.minmax = { 0, 500 }, 0 },
{ "inav_w_z_baro_p", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.w_z_baro_p, .config.minmax = { 0, 10 }, 0 },
@ -554,7 +553,7 @@ const clivalue_t valueTable[] = {
{ "inav_baro_epv", VAR_FLOAT | MASTER_VALUE, &masterConfig.navConfig.inav.baro_epv, .config.minmax = { 0, 9999 }, 0 },
{ "nav_use_midrc_for_althold", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.use_midrc_for_althold, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "nav_throttle_tilt_comp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.throttle_tilt_comp, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "nav_extra_arming_safety", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.extra_arming_safety, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "nav_user_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.user_control_mode, .config.lookup = { TABLE_NAV_USER_CTL_MODE }, 0 },
{ "nav_dterm_cut_hz", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.dterm_cut_hz, .config.minmax = { 0, 100 }, 0 },
{ "nav_wp_radius", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.waypoint_radius, .config.minmax = { 100, 2000 }, 0 },

2
src/main/io/serial_msp.c
View file

@ -657,7 +657,7 @@ void mspInit(serialConfig_t *serialConfig)
if (sensors(SENSOR_BARO) || (isFixedWing && feature(FEATURE_GPS))) {
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
}
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (isFixedWing && feature(FEATURE_GPS)) || (sensors(SENSOR_ACC) && masterConfig.navConfig.inav.enable_dead_reckoning)) {
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (isFixedWing && feature(FEATURE_GPS))) {
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
}
if ((feature(FEATURE_GPS) && sensors(SENSOR_ACC) && sensors(SENSOR_MAG) && (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR))) || (isFixedWing && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {

17
src/main/mw.c
View file

@ -258,14 +258,19 @@ void annexCode(void)
}
if (isCalibrating()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#if defined(NAV)
if (naivationBlockArming()) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#endif
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
warningLedFlash();
}
warningLedUpdate();