From af6d5ee76d54656e232765e0428439aef4f7aecc Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Mon, 18 Nov 2019 22:11:26 +0000 Subject: [PATCH] add rth home offset option --- src/main/fc/settings.yaml | 9 +++++++-- src/main/navigation/navigation.c | 25 ++++++++++++++++++------- src/main/navigation/navigation.h | 4 +++- 3 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 835f133178..aff03e1e44 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -80,7 +80,7 @@ tables: - name: debug_modes values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", - "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", + "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "ERPM"] - name: async_mode values: ["NONE", "GYRO", "ALL"] @@ -1442,7 +1442,12 @@ groups: - name: nav_rth_home_altitude field: general.rth_home_altitude max: 65000 - + - name: nav_rth_home_offset_distance + field: general.rth_home_offset_distance + max: 650 + - name: nav_rth_home_offset_direction + field: general.rth_home_offset_direction + max: 359 - name: nav_mc_bank_angle field: mc.max_bank_angle min: 15 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d20073bcf6..a54f66f65c 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -56,7 +56,6 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" - // Multirotors: #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) @@ -80,7 +79,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS]; PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -115,7 +114,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_home_altitude = 0, // altitude in centimeters .rth_abort_threshold = 50000, // centimeters - 500m should be safe for all aircraft .max_terrain_follow_altitude = 100, // max altitude in centimeters in terrain following mode - }, + .rth_home_offset_distance = 0, // Distance offset from GPS established home to "safe" position used for RTH (metre, 0 disables) + .rth_home_offset_direction = 0, // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance) + }, // MC-specific .mc = { @@ -129,7 +130,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_timeout = 750, // Timout boost after 750ms .braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s .braking_boost_disengage_speed = 100, // Disable boost at 1m/s - .braking_bank_angle = 40, // Max braking angle + .braking_bank_angle = 40, // Max braking angle .posDecelerationTime = 120, // posDecelerationTime * 100 .posResponseExpo = 10, // posResponseExpo * 100 }, @@ -2220,6 +2221,7 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void) /*----------------------------------------------------------- * Update home position, calculate distance and bearing to home *-----------------------------------------------------------*/ + void updateHomePosition(void) { // Disarmed and have a valid position, constantly update home @@ -2238,7 +2240,15 @@ void updateHomePosition(void) break; } if (setHome) { - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + if (navConfig()->general.rth_home_offset_distance != 0) { // apply user defined offset + fpVector3_t newHome; + newHome.x = posControl.actualState.abs.pos.x + 100 * navConfig()->general.rth_home_offset_distance * cos_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction)); + newHome.y = posControl.actualState.abs.pos.y + 100 * navConfig()->general.rth_home_offset_distance * sin_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction)); + newHome.z = posControl.actualState.abs.pos.z; + setHomePosition(&newHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + } else { + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + } } } } @@ -2556,6 +2566,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) // WP #0 - special waypoint - HOME if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) { // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly + geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL); } @@ -3134,7 +3145,7 @@ void navigationUsePIDs(void) // Initialize position hold P-controller for (int axis = 0; axis < 2; axis++) { navPidInit( - &posControl.pids.pos[axis], + &posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f, 0.0f, 0.0f, @@ -3152,7 +3163,7 @@ void navigationUsePIDs(void) // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z navPidInit( - &posControl.pids.pos[Z], + &posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f, 0.0f, 0.0f, diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d8e81ca16d..ccbf5b293f 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -32,7 +32,7 @@ /* GPS Home location data */ extern gpsLocation_t GPS_home; -extern uint32_t GPS_distanceToHome; // distance to home point in meters +extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees extern bool autoThrottleManuallyIncreased; @@ -171,6 +171,8 @@ typedef struct navConfig_s { uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode + uint16_t rth_home_offset_distance; // Distance offset from GPS established home to "safe" position used for RTH (metre, 0 disables) + uint16_t rth_home_offset_direction; // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance) } general; struct {