ArduPlane: Changes to support Plane follow lua script

ArduPlane/GCS_Mavlink.cpp

@@ -863,15 +863,17 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
     if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
         (plane.control_mode == &plane.mode_guided)) {
         plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
-
+#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
+        plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
+#endif
+
         // add home alt if needed
         if (requested_position.relative_alt) {
             requested_position.alt += plane.home.alt;
             requested_position.relative_alt = 0;
         }
 
         plane.set_guided_WP(requested_position);
-
         // Loiter radius for planes. Positive radius in meters, direction is controlled by Yaw (param4) value, parsed above
         if (!isnan(packet.param3) && packet.param3 > 0) {
             plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw);
@@ -973,23 +975,27 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
 
         float new_target_heading = radians(wrap_180(packet.param2));
 
-        // course over ground
-        if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int
+        switch(HEADING_TYPE(packet.param1)) {
+        case HEADING_TYPE_COURSE_OVER_GROUND:
             plane.guided_state.target_heading_type = GUIDED_HEADING_COG;
             plane.prev_WP_loc = plane.current_loc;
-        // normal vehicle heading
-        } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int
+            break;
+        case HEADING_TYPE_HEADING:
             plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING;
-        } else {
-            //  MAV_RESULT_DENIED  means Command is invalid (is supported but has invalid parameters).
+            break;
+        case HEADING_TYPE_DEFAULT:
+            plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
+            return MAV_RESULT_ACCEPTED;
+        default:
             return MAV_RESULT_DENIED;
         }
 
         plane.g2.guidedHeading.reset_I();
-
+        
         plane.guided_state.target_heading = new_target_heading;
-        plane.guided_state.target_heading_accel_limit = MAX(packet.param3, 0.05f);
+        plane.guided_state.target_heading_accel_limit = MAX(is_zero(packet.param3)? 10.0f : packet.param3 , 10.0f); // the, previous limit of 0.05 was 0.29 degrees, not very useful
         plane.guided_state.target_heading_time_ms = AP_HAL::millis();
+
         return MAV_RESULT_ACCEPTED;
     }
   }

ArduPlane/commands.cpp

@@ -87,7 +87,9 @@ void Plane::set_guided_WP(const Location &loc)
 
     // used to control FBW and limit the rate of climb
     // -----------------------------------------------
-    set_target_altitude_current();
+    if (!control_mode->is_guided_mode()) {
+        set_target_altitude_current();
+    }
 
     setup_glide_slope();
     setup_turn_angle();

ArduPlane/mode.cpp

@@ -171,7 +171,6 @@ bool Mode::is_vtol_man_throttle() const
 void Mode::update_target_altitude()
 {
     Location target_location;
-
     if (plane.landing.is_flaring()) {
         // during a landing flare, use TECS_LAND_SINK as a target sink
         // rate, and ignores the target altitude

ArduPlane/mode_guided.cpp

@@ -1,6 +1,9 @@
 #include "mode.h"
 #include "Plane.h"
 
+// In the previous version this defaults to 3000 which limits Guided from doing timely updates
+#define LIMIT_ON_GUIDED_UPDATE_FREQUENCY 25
+
 bool ModeGuided::_enter()
 {
     plane.guided_throttle_passthru = false;
@@ -38,7 +41,7 @@ void ModeGuided::update()
 
     // Received an external msg that guides roll in the last 3 seconds?
     if (plane.guided_state.last_forced_rpy_ms.x > 0 &&
-            millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
+        millis() - plane.guided_state.last_forced_rpy_ms.x < LIMIT_ON_GUIDED_UPDATE_FREQUENCY) {
         plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
         plane.update_load_factor();
 
@@ -76,7 +79,7 @@ void ModeGuided::update()
     }
 
     if (plane.guided_state.last_forced_rpy_ms.y > 0 &&
-            millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
+        millis() - plane.guided_state.last_forced_rpy_ms.y < LIMIT_ON_GUIDED_UPDATE_FREQUENCY) {
         plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
     } else {
         plane.calc_nav_pitch();
@@ -89,7 +92,7 @@ void ModeGuided::update()
 
     }  else if (plane.aparm.throttle_cruise > 1 &&
             plane.guided_state.last_forced_throttle_ms > 0 &&
-            millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
+            millis() - plane.guided_state.last_forced_throttle_ms < LIMIT_ON_GUIDED_UPDATE_FREQUENCY) {
         // Received an external msg that guides throttle in the last 3 seconds?
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);
 

libraries/AP_Follow/AP_Follow.cpp

@@ -29,18 +29,19 @@
 
 extern const AP_HAL::HAL& hal;
 
-#define AP_FOLLOW_TIMEOUT_MS    3000    // position estimate timeout after 1 second
 #define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we have not heard from them in 10 seconds
 
 #define AP_FOLLOW_OFFSET_TYPE_NED       0   // offsets are in north-east-down frame
 #define AP_FOLLOW_OFFSET_TYPE_RELATIVE  1   // offsets are relative to lead vehicle's heading
 
-#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE  1 // relative altitude is used by default   
+#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE  1 // home relative altitude is used by default   
 
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
-#define AP_FOLLOW_ALT_TYPE_DEFAULT 0
+#define AP_FOLLOW_ALT_TYPE_DEFAULT       0 // absolute/AMSL altitude
+#define AP_FOLLOW_ALTITUDE_TYPE_ORIGIN   2 // origin relative altitude
+#define AP_FOLLOW_ALTITUDE_TYPE_TERRAIN  3 // terrain relative altitude
 #else
 #define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
 #endif
@@ -128,8 +129,8 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 #if !(APM_BUILD_TYPE(APM_BUILD_Rover)) 
     // @Param: _ALT_TYPE
     // @DisplayName: Follow altitude type
-    // @Description: Follow altitude type
-    // @Values: 0:absolute, 1:relative
+    // @Description: Follow altitude type 
+    // @Values: 0:absolute, 1:relative, 2:origin (not used) 3:terrain (plane)
     // @User: Standard
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
@@ -141,6 +142,16 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _TIMEOUT
+    // @DisplayName: Follow timeout
+    // @Description: Follow position estimate timeout after x milliseconds
+    // @User: Standard
+    // @Units: milliseconds
+#if (APM_BUILD_TYPE(APM_BUILD_ArduPlane)) 
+    AP_GROUPINFO("_TIMEOUT", 12, AP_Follow, _timeout, 600),
+#else
+    AP_GROUPINFO("_TIMEOUT", 12, AP_Follow, _timeout, 3000),
+#endif
     AP_GROUPEND
 };
 
@@ -174,7 +185,7 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne
     }
 
     // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > (uint32_t)_timeout)) {
         return false;
     }
 
@@ -263,7 +274,7 @@ bool AP_Follow::get_target_heading_deg(float &heading) const
     }
 
     // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > (uint32_t)_timeout)) {
         return false;
     }
 
@@ -345,14 +356,35 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
         _target_location.lng = packet.lon;
 
         // select altitude source based on FOLL_ALT_TYPE param 
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+        switch(_alt_type) {
+            case AP_FOLLOW_ALT_TYPE_DEFAULT:
+                _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
+                break;
+            case AP_FOLLOW_ALTITUDE_TYPE_RELATIVE:
+            case AP_FOLLOW_ALTITUDE_TYPE_ORIGIN:
+                _target_location.set_alt_cm(packet.relative_alt * 0.1, static_cast<Location::AltFrame>((int)_alt_type));
+                break;
+            case AP_FOLLOW_ALTITUDE_TYPE_TERRAIN: {
+                /// Altitude comes in AMSL
+                int32_t terrain_altitude_cm;
+                _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
+                // convert the incoming altitude to terrain altitude
+                if(_target_location.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terrain_altitude_cm)) {
+                    _target_location.set_alt_cm(terrain_altitude_cm, Location::AltFrame::ABOVE_TERRAIN);
+                }
+                break;
+            }
+        }
+#else
         if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
             // above home alt
             _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
         } else {
             // absolute altitude
             _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
         }
-
+#endif
         _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
         _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
         _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
@@ -393,8 +425,8 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 
         // FOLLOW_TARGET is always AMSL, change the provided alt to
         // above home if we are configured for relative alt
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
-            !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
+        if (_alt_type != AP_FOLLOW_ALT_TYPE_DEFAULT &&
+            !new_loc.change_alt_frame(static_cast<Location::AltFrame>((int)_alt_type))) {
             return false;
         }
         _target_location = new_loc;
@@ -559,11 +591,42 @@ bool AP_Follow::have_target(void) const
     }
 
     // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > (uint32_t)_timeout)) {
+        gcs().send_text(MAV_SEVERITY_NOTICE, "location timeout %d", (int)(AP_HAL::millis() - _last_location_update_ms));
+        return false;
+    }
+    return true;
+}
+
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+// create a single method to retrieve all the relevant values in one shot for Lua
+/* replaces the following Lua calls
+   target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned()
+   target_location, target_velocity = follow:get_target_location_and_velocity()
+   target_location_offset, target_velocity = follow:get_target_location_and_velocity_ofs()
+   local xy_dist = follow:get_distance_to_target() -- this value is set by get_target_dist_and_vel_ned() - why do I have to know this?
+   local target_heading = follow:get_target_heading_deg()
+*/
+bool AP_Follow::get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs, 
+                                Vector3f &target_vel_ned, Vector3f &target_vel_ned_ofs,
+                                Location &target_loc, Location &target_loc_ofs, 
+                                float &target_dist_ofs
+                                )
+{
+    // The order here is VERY important. This needs to called first because it updates a lot of internal variables
+    if(!get_target_dist_and_vel_ned(dist_ned, dist_with_offs, target_vel_ned)) {
         return false;
     }
+    if(!get_target_location_and_velocity(target_loc,target_vel_ned)) {
+        return false;
+    }   
+    if(!get_target_location_and_velocity_ofs(target_loc_ofs, target_vel_ned_ofs)) {
+        return false;
+    }
+    target_dist_ofs = _dist_to_target;
     return true;
 }
+#endif
 
 namespace AP {
 

libraries/AP_Follow/AP_Follow.h

@@ -26,6 +26,8 @@
 #include <AC_PID/AC_P.h>
 #include <AP_RTC/JitterCorrection.h>
 
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+
 class AP_Follow
 {
 
@@ -112,6 +114,18 @@ class AP_Follow
     // returns true if a follow option enabled
     bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; }
 
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    //
+    // Lua binding combo function
+    //
+    // try to get all the values from a single cycle and return them in a single call to Lua
+    bool get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs, 
+                                Vector3f &target_vel_ned, Vector3f &target_vel_ned_ofs,
+                                Location &target_loc, Location &target_loc_ofs, 
+                                float &target_dist_ofs
+                                );
+#endif
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -153,6 +167,7 @@ class AP_Follow
     AP_Int8     _alt_type;          // altitude source for follow mode
     AC_P        _p_pos;             // position error P controller
     AP_Int16    _options;           // options for mount behaviour follow mode
+    AP_Int32    _timeout;           // position estimate timeout after x milliseconds
 
     // local variables
     uint32_t _last_location_update_ms;  // system time of last position update

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -7,10 +7,12 @@
 
 extern const AP_HAL::HAL& hal;
 
+
 #define AP_MOUNT_UPDATE_DT 0.02     // update rate in seconds.  update() should be called at this rate
 #define AP_MOUNT_POI_REQUEST_TIMEOUT_MS 30000   // POI calculations continue to be updated for this many seconds after last request
 #define AP_MOUNT_POI_RESULT_TIMEOUT_MS  3000    // POI calculations valid for 3 seconds
 #define AP_MOUNT_POI_DIST_M_MAX         10000   // POI calculations limit of 10,000m (10km)
+#define AP_MOUNT_SYSID_TIMEOUT_MS       600     // SYSID position estimate timeout after .6 second, which from testing seems "good"
 
 // Default init function for every mount
 void AP_Mount_Backend::init()
@@ -426,6 +428,9 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli
     // global_position_int.alt is *UP*, so is location.
     _target_sysid_location.set_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE);
     _target_sysid_location_set = true;
+    // keep track of when we last received the update
+    //_target_sysid_update_ms = AP_HAL::millis();
+    _target_sysid_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
     return true;
 }
@@ -889,6 +894,10 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const
     if (!_target_sysid) {
         return false;
     }
+    // don't update the angle if we haven't received a recent update from the target
+    if(AP_HAL::millis() - _target_sysid_update_ms > AP_MOUNT_SYSID_TIMEOUT_MS) {
+        return false;
+    }
     return get_angle_target_to_location(_target_sysid_location, angle_rad);
 }
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -29,6 +29,7 @@
 #include <RC_Channel/RC_Channel.h>
 #include <AP_Camera/AP_Camera_shareddefs.h>
 #include "AP_Mount.h"
+#include <AP_RTC/JitterCorrection.h>
 
 class AP_Mount_Backend
 {
@@ -341,6 +342,9 @@ class AP_Mount_Backend
     uint8_t _target_sysid;          // sysid to track
     Location _target_sysid_location;// sysid target location
     bool _target_sysid_location_set;// true if _target_sysid has been set
+    uint32_t _target_sysid_update_ms;// timestamp when the target_sysid_location was last updated
+    // setup jitter correction with max transport lag of 3s
+    JitterCorrection _jitter{3000};
 
     uint32_t _last_warning_ms;      // system time of last warning sent to GCS