Plane: resync for 4.0.5

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;

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,

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.
+    // @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
+    // @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
 };
 

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[];

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);

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

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);
+        }
+    }
 }
 

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;

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;

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());
 }
 
 /*

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()) {

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