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

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

25
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,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,