diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 62a76132cf..3bda772f69 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -310,6 +310,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: Use terrain following // @Description: This enables terrain following for CRUISE mode, FBWB mode, RTL and for rally points. To use this option you also need to set TERRAIN_ENABLE to 1, which enables terrain data fetching from the GCS, and you need to have a GCS that supports sending terrain data to the aircraft. When terrain following is enabled then CRUISE and FBWB mode will hold height above terrain rather than height above home. In RTL the return to launch altitude will be considered to be a height above the terrain. Rally point altitudes will be taken as height above the terrain. This option does not affect mission items, which have a per-waypoint flag for whether they are height above home or height above the terrain. To use terrain following missions you need a ground station which can set the waypoint type to be a terrain height waypoint when creating the mission. // @Values: 0:Disabled,1:Enabled + // @Bitmask: 0: Enable all modes, 1:FBWB, 2:Cruise, 3:Auto, 4:RTL, 5:Avoid_ADSB, 6:Guided, 7:Loiter, 8:Circle, 9:QRTL, 10:QLand, 11:Qloiter // @User: Standard GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), @@ -1470,6 +1471,10 @@ void Plane::load_parameters(void) } } +#if AP_TERRAIN_AVAILABLE + g.terrain_follow.convert_parameter_width(AP_PARAM_INT8); +#endif + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 566ca8c8a9..017a248bf3 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -470,7 +470,7 @@ public: AP_Float takeoff_pitch_limit_reduction_sec; AP_Int8 level_roll_limit; #if AP_TERRAIN_AVAILABLE - AP_Int8 terrain_follow; + AP_Int32 terrain_follow; AP_Int16 terrain_lookahead; #endif AP_Int16 glide_slope_min; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 491414ae5d..6b37dbcc8e 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -788,6 +788,28 @@ private: // terrain disable for non AUTO modes, set with an RC Option switch bool non_auto_terrain_disable; bool terrain_disabled(); +#if AP_TERRAIN_AVAILABLE + bool terrain_enabled_in_current_mode() const; + enum class terrain_bitmask { + ALL = 1U << 0, + FLY_BY_WIRE_B = 1U << 1, + CRUISE = 1U << 2, + AUTO = 1U << 3, + RTL = 1U << 4, + AVOID_ADSB = 1U << 5, + GUIDED = 1U << 6, + LOITER = 1U << 7, + CIRCLE = 1U << 8, + QRTL = 1U << 9, + QLAND = 1U << 10, + QLOITER = 1U << 11, + }; + struct TerrainLookupTable{ + Mode::Number mode_num; + terrain_bitmask bitmask; + }; + static const TerrainLookupTable Terrain_lookup[]; +#endif // Attitude.cpp void adjust_nav_pitch_throttle(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index abae31545e..734b0d2f8f 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -200,7 +200,7 @@ void Plane::set_target_altitude_current(void) #if AP_TERRAIN_AVAILABLE // also record the terrain altitude if possible float terrain_altitude; - if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) { + if (terrain_enabled_in_current_mode() && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) { target_altitude.terrain_following = true; target_altitude.terrain_alt_cm = terrain_altitude*100; } else { @@ -462,7 +462,7 @@ bool Plane::above_location_current(const Location &loc) void Plane::setup_terrain_target_alt(Location &loc) const { #if AP_TERRAIN_AVAILABLE - if (g.terrain_follow) { + if (terrain_enabled_in_current_mode()) { loc.terrain_alt = true; } #endif @@ -618,7 +618,7 @@ void Plane::rangefinder_terrain_correction(float &height) #if AP_TERRAIN_AVAILABLE if (!g.rangefinder_landing || flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND || - g.terrain_follow == 0) { + !terrain_enabled_in_current_mode()) { return; } float terrain_amsl1, terrain_amsl2; @@ -690,7 +690,7 @@ void Plane::rangefinder_height_update(void) #if AP_TERRAIN_AVAILABLE // if we are terrain following then correction is based on terrain data float terrain_altitude; - if ((target_altitude.terrain_following || g.terrain_follow) && + if ((target_altitude.terrain_following || terrain_enabled_in_current_mode()) && terrain.height_above_terrain(terrain_altitude, true)) { correction = terrain_altitude - rangefinder_state.height_estimate; } @@ -730,3 +730,41 @@ bool Plane::terrain_disabled() } +/* + Check if terrain following is enabled for the current mode + */ +#if AP_TERRAIN_AVAILABLE +const Plane::TerrainLookupTable Plane::Terrain_lookup[] = { + {Mode::Number::FLY_BY_WIRE_B, terrain_bitmask::FLY_BY_WIRE_B}, + {Mode::Number::CRUISE, terrain_bitmask::CRUISE}, + {Mode::Number::AUTO, terrain_bitmask::AUTO}, + {Mode::Number::RTL, terrain_bitmask::RTL}, + {Mode::Number::AVOID_ADSB, terrain_bitmask::AVOID_ADSB}, + {Mode::Number::GUIDED, terrain_bitmask::GUIDED}, + {Mode::Number::LOITER, terrain_bitmask::LOITER}, + {Mode::Number::CIRCLE, terrain_bitmask::CIRCLE}, + {Mode::Number::QRTL, terrain_bitmask::QRTL}, + {Mode::Number::QLAND, terrain_bitmask::QLAND}, + {Mode::Number::QLOITER, terrain_bitmask::QLOITER}, +}; + +bool Plane::terrain_enabled_in_current_mode() const +{ + // Global enable + if ((g.terrain_follow.get() & int32_t(terrain_bitmask::ALL)) != 0) { + return true; + } + + // Specific enable + for (const struct TerrainLookupTable entry : Terrain_lookup) { + if (entry.mode_num == control_mode->mode_number()) { + if ((g.terrain_follow.get() & int32_t(entry.bitmask)) != 0) { + return true; + } + break; + } + } + + return false; +} +#endif