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

add rth home offset option

Browse source
This commit is contained in:
Jonathan Hudson 2019-11-18 22:11:26 +00:00
parent c58b0247ed
commit af6d5ee76d
3 changed files with 28 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/fc/settings.yaml
View file

@ -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

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

@ -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,6 +114,8 @@ 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
@ -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,10 +2240,18 @@ void updateHomePosition(void)
break;
}
if (setHome) {
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());
}
}
}
}
else {
static bool isHomeResetAllowed = false;
@ -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);
}

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

@ -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 {