diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index bf5e58eb0d..2b8c2f092c 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1392,10 +1392,14 @@ static void update_navigation() } } -static void update_flight_stage(AP_SpdHgtControl::FlightStage fs) { +/* + set the flight stage + */ +static void set_flight_stage(AP_SpdHgtControl::FlightStage fs) +{ //if just now entering land flight stage if (fs == AP_SpdHgtControl::FLIGHT_LAND_APPROACH && - flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { + flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable == 1) { @@ -1406,7 +1410,6 @@ static void update_flight_stage(AP_SpdHgtControl::FlightStage fs) { } } #endif - } flight_stage = fs; @@ -1421,21 +1424,29 @@ static void update_alt() geofence_check(true); + update_flight_stage(); +} + +/* + recalculate the flight_stage + */ +static void update_flight_stage(void) +{ // Update the speed & height controller states if (auto_throttle_mode && !throttle_suppressed) { if (control_mode==AUTO) { if (auto_state.takeoff_complete == false) { - update_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); + set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && auto_state.land_complete == true) { - update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); + set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); + set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); } else { - update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); + set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } } else { - update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); + set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 1781816435..20c969f375 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -119,6 +119,7 @@ public: k_param_terrain, k_param_terrain_follow, k_param_stab_pitch_down_cd, + k_param_glide_slope_threshold, // 100: Arming parameters k_param_arming = 100, @@ -452,6 +453,7 @@ public: #if AP_TERRAIN_AVAILABLE AP_Int8 terrain_follow; #endif + AP_Int16 glide_slope_threshold; // RC channels RC_Channel rc_1; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index df295a5921..b6d1e1168e 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -113,6 +113,15 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(stab_pitch_down_cd, "STAB_PITCH_DN_CD", 200), + // @Param: GLIDE_SLOPE_MIN + // @DisplayName: Glide slope threshold + // @Description: This controls the minimum altitude change for a waypoint before a glide slope will be used instead of an immediate altitude change. The default value is 15 meters, which helps to smooth out waypoint missions where small altitude changes happen near waypoints. If you don't want glide slopes to be used in missions then you can set this to zero, which will disable glide slope calculations. Otherwise you can set it to a minimum number of meters of altitude error to the destination waypoint before a glide slope will be used to change altitude. + // @Range: 0 1000 + // @Increment: 1 + // @Units: meters + // @User: Advanced + GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_MIN", 15), + // @Param: STICK_MIXING // @DisplayName: Stick Mixing // @Description: When enabled, this adds user stick input to the control surfaces in auto modes, allowing the user to have some degree of flight control without changing modes. There are two types of stick mixing available. If you set STICK_MIXING to 1 then it will use "fly by wire" mixing, which controls the roll and pitch in the same way that the FBWA mode does. This is the safest option if you usually fly ArduPlane in FBWA or FBWB mode. If you set STICK_MIXING to 2 then it will enable direct mixing mode, which is what the STABILIZE mode uses. That will allow for much more extreme maneuvers while in AUTO mode. diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde index d2e0986b83..0216adaa86 100644 --- a/ArduPlane/altitude.pde +++ b/ArduPlane/altitude.pde @@ -325,13 +325,17 @@ static void set_offset_altitude_location(const Location &loc) } #endif - // if we are within 15 meters of the target altitude then reset - // the offset to not use a glide slope. This allows for more - // accurate flight of missions where the aircraft may lose or gain - // a bit of altitude near waypoint turn points due to local - // terrain changes - if (labs(target_altitude.offset_cm) < 1500) { - target_altitude.offset_cm = 0; + if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH && + flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL) { + // if we are within GLIDE_SLOPE_MIN meters of the target altitude + // then reset the offset to not use a glide slope. This allows for + // more accurate flight of missions where the aircraft may lose or + // gain a bit of altitude near waypoint turn points due to local + // terrain changes + if (g.glide_slope_threshold <= 0 || + labs(target_altitude.offset_cm)*0.01f < g.glide_slope_threshold) { + target_altitude.offset_cm = 0; + } } } diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 34636394d7..fe1922c64b 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -79,6 +79,7 @@ static void set_guided_WP(void) // ----------------------------------------------- set_target_altitude_current(); + update_flight_stage(); setup_glide_slope(); setup_turn_angle(); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index a44a446868..d850a65df5 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -261,6 +261,7 @@ static void do_RTL(void) loiter.direction = 1; } + update_flight_stage(); setup_glide_slope(); setup_turn_angle(); @@ -643,6 +644,7 @@ static void exit_mission_callback() rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM; setup_terrain_target_alt(auto_rtl_command.content.location); + update_flight_stage(); setup_glide_slope(); setup_turn_angle(); start_command(auto_rtl_command);