From fea5c6ea10b0f22f4cd91f5c3cfdb7da1c4fea67 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Oct 2020 10:13:33 +1000 Subject: [PATCH] Plane: resync for 4.0.5 --- ArduPlane/AP_Arming.cpp | 17 ++++++++++++++++- ArduPlane/ArduPlane.cpp | 15 +++++++++++++-- ArduPlane/Parameters.cpp | 15 ++++++++++++--- ArduPlane/Parameters.h | 3 +++ ArduPlane/Plane.h | 10 ++++++++++ ArduPlane/RC_Channel.cpp | 2 +- ArduPlane/mode_rtl.cpp | 15 +++++++++++++++ ArduPlane/mode_takeoff.cpp | 2 +- ArduPlane/navigation.cpp | 33 +++++++++++++++++++++++++++++++-- ArduPlane/quadplane.cpp | 4 ++++ ArduPlane/radio.cpp | 6 ++++-- ArduPlane/version.h | 10 +++------- 12 files changed, 113 insertions(+), 19 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index af74e260fd..e535984bff 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -58,7 +58,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) } if (plane.channel_throttle->get_reverse() && - plane.g.throttle_fs_enabled && + Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled && plane.g.throttle_fs_value < plane.channel_throttle->get_radio_max()) { check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle"); @@ -88,6 +88,21 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) } } +#if AP_TERRAIN_AVAILABLE + if (plane.g.terrain_follow || plane.mission.contains_terrain_relative()) { + // check terrain data is loaded and healthy + uint16_t terr_pending=0, terr_loaded=0; + plane.terrain.get_statistics(terr_pending, terr_loaded); + if (plane.terrain.status() != AP_Terrain::TerrainStatusOK) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "terrain data unhealthy"); + ret = false; + } else if (terr_pending != 0) { + check_failed(ARMING_CHECK_PARAMETERS, display_failure, "waiting for terrain data"); + ret = false; + } + } +#endif + if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) { check_failed(display_failure, "No mission loaded"); ret = false; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index cfb5eca297..90c7cda6ba 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -52,7 +52,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(afs_fs_check, 10, 100), #endif SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500), - SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 500), + SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750), SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150), SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300), SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150), @@ -462,6 +462,7 @@ void Plane::update_navigation() // we've reached the RTL point, see if we have a landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO + mission.set_force_resume(true); set_mode(mode_auto, ModeReason::UNKNOWN); } @@ -474,6 +475,7 @@ void Plane::update_navigation() // Go directly to the landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO + mission.set_force_resume(true); set_mode(mode_auto, ModeReason::UNKNOWN); } @@ -582,7 +584,16 @@ void Plane::update_alt() } #endif - SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), + float target_alt = relative_target_altitude_cm(); + + if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) { + // ensure we do the initial climb in RTL. We add an extra + // 10m in the demanded height to push TECS to climb + // quickly + target_alt = MAX(target_alt, prev_WP_loc.alt + (g2.rtl_climb_min+10)*100); + } + + SpdHgt_Controller->update_pitch_throttle(target_alt, target_airspeed_cm, flight_stage, distance_beyond_land_wp, diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 4719e08e94..87e67dbb59 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -460,10 +460,10 @@ const AP_Param::Info Plane::var_info[] = { // @Param: THR_FAILSAFE // @DisplayName: Throttle and RC Failsafe Enable - // @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver. - // @Values: 0:Disabled,1:Enabled + // @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver. A value of 2 means that the RC input won't be used when throttle goes below the THR_FS_VALUE, but it won't trigger a failsafe + // @Values: 0:Disabled,1:Enabled,2:EnabledNoFailsafe // @User: Standard - GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", 1), + GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", int(ThrFailsafe::Enabled)), // @Param: THR_FS_VALUE @@ -1262,6 +1262,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0), + // @Param: RTL_CLIMB_MIN + // @DisplayName: RTL minimum climb + // @Description: The vehicle will climb this many m during the initial climb portion of the RTL. During this time the roll will be limited to LEVEL_ROLL_LIMIT degrees. + // @Units: m + // @Range: 0 30 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0), + AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ef5a76c7ad..43d37ef47a 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -569,6 +569,9 @@ public: AP_Float fwd_thr_batt_voltage_max; AP_Float fwd_thr_batt_voltage_min; AP_Int8 fwd_thr_batt_idx; + + // min initial climb in RTL + AP_Int16 rtl_climb_min; }; extern const AP_Param::Info var_info[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 55f3bcb34d..abc6d36e06 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -746,6 +746,10 @@ private: uint32_t last_trim_save; } auto_trim; + struct { + bool done_climb; + } rtl; + // last time home was updated while disarmed uint32_t last_home_update_ms; @@ -1052,6 +1056,12 @@ private: static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); + enum class ThrFailsafe { + Disabled = 0, + Enabled = 1, + EnabledNoFS = 2 + }; + public: void mavlink_delay_cb(); void failsafe_check(void); diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 98f7a32ec3..5aab80796f 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -18,7 +18,7 @@ int8_t RC_Channels_Plane::flight_mode_channel_number() const bool RC_Channels_Plane::has_valid_input() const { - if (plane.failsafe.rc_failsafe) { + if (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe) { return false; } if (plane.failsafe.throttle_counter != 0) { diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 9ad2370222..ebe218bfa0 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -8,6 +8,7 @@ bool ModeRTL::_enter() plane.auto_navigation_mode = true; plane.prev_WP_loc = plane.current_loc; plane.do_RTL(plane.get_RTL_altitude()); + plane.rtl.done_climb = false; return true; } @@ -17,5 +18,19 @@ void ModeRTL::update() plane.calc_nav_roll(); plane.calc_nav_pitch(); plane.calc_throttle(); + + if (plane.g2.rtl_climb_min > 0) { + /* + when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT + until we have climbed by RTL_CLIMB_MIN meters + */ + if (!plane.rtl.done_climb && (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min) { + plane.rtl.done_climb = true; + } + if (!plane.rtl.done_climb) { + plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100); + plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd); + } + } } diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index c168234b2c..3598b59156 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -65,7 +65,7 @@ void ModeTakeoff::update() { if (!takeoff_started) { // see if we will skip takeoff as already flying - if (plane.is_flying() && plane.ahrs.groundspeed() > 3) { + if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && plane.ahrs.groundspeed() > 3) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling"); plane.prev_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index a1366dadb6..c81cc7d004 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -36,8 +36,9 @@ void Plane::loiter_angle_update(void) const int32_t target_bearing_cd = nav_controller->target_bearing_cd(); int32_t loiter_delta_cd; + const bool reached_target = reached_loiter_target(); - if (loiter.sum_cd == 0 && !reached_loiter_target()) { + if (loiter.sum_cd == 0 && !reached_target) { // we don't start summing until we are doing the real loiter loiter_delta_cd = 0; } else if (loiter.sum_cd == 0) { @@ -53,7 +54,35 @@ void Plane::loiter_angle_update(void) loiter_delta_cd = wrap_180_cd(loiter_delta_cd); loiter.sum_cd += loiter_delta_cd * loiter.direction; - if (labs(current_loc.alt - next_WP_loc.alt) < 500) { + bool reached_target_alt = false; + + if (reached_target) { + // once we reach the position target we start checking the + // altitude target + bool terrain_status_ok = false; +#if AP_TERRAIN_AVAILABLE + /* + if doing terrain following then we check against terrain + target, fetch the terrain information + */ + float altitude_agl = 0; + if (target_altitude.terrain_following) { + if (terrain.status() == AP_Terrain::TerrainStatusOK && + terrain.height_above_terrain(altitude_agl, true)) { + terrain_status_ok = true; + } + } + if (terrain_status_ok && + fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) { + reached_target_alt = true; + } else +#endif + if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) { + reached_target_alt = true; + } + } + + if (reached_target_alt) { loiter.reached_target_alt = true; loiter.unable_to_acheive_target_alt = false; loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c72f6ad710..3e94aca66f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2367,6 +2367,10 @@ void QuadPlane::setup_target_position(void) // setup vertical speed and acceleration pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control->set_max_accel_z(pilot_accel_z); + + // setup horizontal speed and acceleration + pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); + pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration()); } /* diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index f30995cbc9..5d4d9a1f09 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -179,6 +179,8 @@ void Plane::read_radio() { if (!rc().read_input()) { control_failsafe(); + airspeed_nudge_cm = 0; + throttle_nudge = 0; return; } @@ -280,7 +282,7 @@ void Plane::control_failsafe() } } - if(g.throttle_fs_enabled == 0) { + if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) { return; } @@ -373,7 +375,7 @@ bool Plane::trim_radio() */ bool Plane::rc_throttle_value_ok(void) const { - if (!g.throttle_fs_enabled) { + if (ThrFailsafe(g.throttle_fs_enabled.get()) == ThrFailsafe::Disabled) { return true; } if (channel_throttle->get_reverse()) { diff --git a/ArduPlane/version.h b/ArduPlane/version.h index abe4a32bdc..f4111e1f17 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,17 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.0.6beta1" +#define THISFIRMWARE "ArduPlane V4.0.6" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,0,6,FIRMWARE_VERSION_TYPE_BETA +#define FIRMWARE_VERSION 4,0,6,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 4 #define FW_MINOR 0 #define FW_PATCH 6 -#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA - - - - +#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL