From 9b69f7941655c1dc1eebac5def609e0a5f91fbca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Jun 2024 07:35:12 +1000 Subject: [PATCH 01/20] GCS_MAVLink: added GCS semaphore allows for multi-thread access to a wider variety of functions --- libraries/GCS_MAVLink/GCS.h | 11 ++++++----- libraries/GCS_MAVLink/GCS_Common.cpp | 7 +++++-- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fc677dfec0f2e..879c2fc4e0e49 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1148,6 +1148,11 @@ class GCS return _singleton; } + // allow threads to lock against GCS operations + HAL_Semaphore &get_semaphore(void) { + return _sem; + } + virtual uint32_t custom_mode() const = 0; virtual MAV_TYPE frame_type() const = 0; virtual const char* frame_string() const { return nullptr; } @@ -1160,13 +1165,8 @@ class GCS class StatusTextQueue : public ObjectArray { public: using ObjectArray::ObjectArray; - HAL_Semaphore &semaphore() { return _sem; } void prune(); private: - // a lock for the statustext queue, to make it safe to use send_text() - // from multiple threads - HAL_Semaphore _sem; - uint32_t last_prune_ms; }; @@ -1301,6 +1301,7 @@ class GCS private: static GCS *_singleton; + HAL_Semaphore _sem; void create_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, AP_HAL::UARTDriver &uart); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 05c2d8f85c81e..d9bb6720a0213 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2408,10 +2408,12 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, u { char first_piece_of_text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]{}; + // ensure send_text from multiple threads is safe + WITH_SEMAPHORE(_sem); + do { // send_text can be called from multiple threads; we must // protect the "text" member with _statustext_sem - WITH_SEMAPHORE(_statustext_queue.semaphore()); hal.util->vsnprintf(statustext_printf_buffer, sizeof(statustext_printf_buffer), fmt, arg_list); memcpy(first_piece_of_text, statustext_printf_buffer, ARRAY_SIZE(first_piece_of_text)-1); @@ -2514,7 +2516,7 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, u void GCS::service_statustext(void) { - WITH_SEMAPHORE(_statustext_queue.semaphore()); + WITH_SEMAPHORE(_sem); if (_statustext_queue.is_empty()) { // nothing to do @@ -2671,6 +2673,7 @@ void GCS::update_send() void GCS::update_receive(void) { + WITH_SEMAPHORE(_sem); for (uint8_t i=0; iupdate_receive(); } From 1d95dfc7fe10d70f6dea392e6d894a4fa69c2b2d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 23 Jun 2024 19:37:51 +1000 Subject: [PATCH 02/20] GCS_MAVLink: added lua_command_int_packet() for lua access to MAV_CMD_xxx --- libraries/GCS_MAVLink/GCS.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index c4a11f666d55d..cb9fdbe53c364 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -409,9 +409,6 @@ bool GCS_MAVLINK::check_payload_size(uint16_t max_payload_len) #if AP_SCRIPTING_ENABLED /* lua access to command_int - - Note that this is called with the AP_Scheduler lock, ensuring the - main thread does not race with a lua command_int */ MAV_RESULT GCS::lua_command_int_packet(const mavlink_command_int_t &packet) { @@ -423,6 +420,8 @@ MAV_RESULT GCS::lua_command_int_packet(const mavlink_command_int_t &packet) // we need a dummy message for some calls mavlink_message_t msg {}; + WITH_SEMAPHORE(_sem); + return ch->handle_command_int_packet(packet, msg); } #endif // AP_SCRIPTING_ENABLED From bdbf600f91838abe00d9f6b583d57979bcc184dd Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 29 Jun 2024 21:30:08 -0600 Subject: [PATCH 03/20] DRAFT: add plane_follow.lua applet --- ArduPlane/GCS_Mavlink.cpp | 3 +- .../AP_Scripting/applets/plane_follow.lua | 417 ++++++++++++++++++ libraries/AP_Scripting/docs/docs.lua | 31 +- .../generator/description/bindings.desc | 3 + 4 files changed, 447 insertions(+), 7 deletions(-) create mode 100644 libraries/AP_Scripting/applets/plane_follow.lua diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 527f8dd68c285..7c8d547091e35 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -858,7 +858,8 @@ 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); - + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + // add home alt if needed if (requested_position.relative_alt) { requested_position.alt += plane.home.alt; diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua new file mode 100644 index 0000000000000..13f7cda4084ac --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -0,0 +1,417 @@ +--[[ + support follow "mode" in plane. Theis will actually use GUIDED mode with + a scripting switch to allow guided to track the vehicle id in FOLL_SYSID +--]] + +SCRIPT_VERSION = "4.6.0-003" +SCRIPT_NAME = "Plane Follow" +SCRIPT_NAME_SHORT = "PFollow" + +REFRESH_RATE = 0.1 -- in seconds, so 10Hz +LOST_TARGET_TIMEOUT = 5 / REFRESH_RATE -- 5 seconds +AIRSPEED_GAIN = 1.2 +OVERSHOOT_ANGLE = 45 + +local PARAM_TABLE_KEY = 83 +local PARAM_TABLE_PREFIX = "FOLL_" + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3} +MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } +MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } +MAV_HEADING_TYPE = { COG = 0, HEADING = 1} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points + +local MODE_GUIDED = 15 +local MODE_RTL = 11 +local MODE_LOITER = 12 + +local current_location = ahrs:get_location() +local now = millis():tofloat() * 0.001 +local now_too_close = millis():tofloat() * 0.001 +local now_overshot = millis():tofloat() * 0.001 +local now_distance = millis():tofloat() * 0.001 +local follow_enabled = false +local too_close_follow_up = 0 +local lost_target_countdown = LOST_TARGET_TIMEOUT +DISTANCE_LOOKAHEAD_SECONDS = 10 + +-- bind a parameter to a variable +function bind_param(name) + return Parameter(name) +end + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- setup follow mode specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table') + +-- This uses the exisitng FOLL_* parameters and just adds a couple specific to this script +-- but because most of the logic is already in AP_Follow (called by binding to follow:) there +-- is no need to access them in the scriot +-- FOLL_ + +-- we need these existing FOLL_ parametrs +FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE') + +-- Add these FOLL_ parameters specifically for this script +--[[ + // @Param: FOLL_FAIL_MODE + // @DisplayName: Plane Follow lost target mode + // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). + // @User: Standard +--]] +FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, MODE_RTL) + +--[[ + // @Param: FOLL_EXIT_MODE + // @DisplayName: Plane Follow exit mode + // @Description: Mode to switch to when follow mode is exited normally + // @User: Standard +--]] +FOLL_EXIT_MODE = bind_add_param('EXIT_MODE', 8, MODE_LOITER) + +--[[ + // @Param: FOLL_ACT_FN + // @DisplayName: Plane Follow Scripting ActivationFunction + // @Description: Setting an RC channel's _OPTION to this value will use it for Plane Follow enable/disable + // @Range: 300 307 +--]] +FOLL_ACT_FN = bind_add_param("ACT_FN", 9, 301) + +--[[ +create a NaN value +--]] +local function NaN() + return 0/0 +end + +local now_altitude = millis():tofloat() * 0.001 +local function set_vehicle_target_altitude(target) + if math.floor(now) ~= math.floor(now_altitude) then + now_altitude = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": alt: " .. target.alt) + end + + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { frame = MAV_FRAME.GLOBAL, + z = target.alt }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") + end +end + + +local function set_vehicle_target_location(target) + if math.floor(now) ~= math.floor(now_altitude) then + now_altitude = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. " location : alt: " .. target.alt) + end + + if not gcs:run_command_int(MAV_CMD_INT.DO_REPOSITION, { frame = MAV_FRAME.GLOBAL, + p1 = target.groundspeed or -1, + p4 = target.yaw or NaN(), + x = target.lat, + y = target.lng, + z = target.alt }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_REPOSITION returned false") + end + if target.alt > 0 then + set_vehicle_target_altitude({alt = target.alt}) + end +end + +local now_heading = millis():tofloat() * 0.001 +local function set_vehicle_heading(heading) + local heading_type = heading.type or MAV_HEADING_TYPE.HEADING + + if math.floor(now) ~= math.floor(now_heading) then + now_heading = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": heading: " .. heading.heading) + end + + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL, + p1 = heading_type, p2 = heading.heading }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_HEADING returned false") + end +end + +local function set_vehicle_speed(speed) + local new_speed = speed.speed or 0.0 + local speed_type = speed.type or MAV_SPEED_TYPE.AIRSPEED + local throttle = speed.throttle or 0.0 + local slew = speed.slew or 0.0 + local mode = vehicle:get_mode() + if speed_type == MAV_SPEED_TYPE.AIRSPEED and speed.slew ~= 0 and mode == MODE_GUIDED then + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, p2 = new_speed, p3 = slew }) then + + -- if not gcs:command_int(MAV_FRAME.GLOBAL, MAV_CMD_INT.GUIDED_CHANGE_SPEED, { speed_type , new_speed, slew, 0, 0, 0, 0} ) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_SPEED returned false") + end + else + if not gcs:run_command_int(MAV_CMD_INT.DO_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, p2 = new_speed, p3 = throttle }) then + -- if not gcs:command_int(MAV_FRAME.GLOBAL, MAV_CMD_INT.DO_CHANGE_SPEED, { speed.type or MAV_SPEED_TYPE.AIRSPEED , new_speed, throttle, 0, 0, 0, 0} ) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_CHANGE_SPEED returned false") + end + end +end + +local last_follow_active_state = rc:get_aux_cached(FOLL_ACT_FN:get()) + +AIRSPEED_MIN = bind_param('AIRSPEED_MIN') +AIRSPEED_MAX = bind_param('AIRSPEED_MAX') +AIRSPEED_CRUISE = bind_param('AIRSPEED_CRUISE') +FOLL_OFS_Y = bind_param('FOLL_OFS_Y') +WP_LOITER_RAD = bind_param('WP_LOITER_RAD') +--[[ + return true if we are in a state where follow can apply +--]] +local function follow_active() + local mode = vehicle:get_mode() + + if mode == MODE_GUIDED then + if follow_enabled then + if follow:have_target() then + return true + else + return false + end + end + end + + return false +end + +--[[ + check for user activating follow using an RC switch set HIGH +--]] +local function follow_check() + local active_state = rc:get_aux_cached(FOLL_ACT_FN:get()) + if (active_state ~= last_follow_active_state) then + if( active_state == 0) then + if follow_enabled then + -- Follow disabled - return to EXIT mode + vehicle:set_mode(FOLL_EXIT_MODE:get()) + follow_enabled = false + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled") + end + elseif (active_state == 2) then + if not (arming:is_armed()) then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": must be armed") + end + -- Follow enabled - switch to guided mode + vehicle:set_mode(MODE_GUIDED) + -- Force Guided Mode to not loiter by setting a tiny loiter radius. Otherwise Guide Mode will try loiter around the target vehichle when it gets inside WP_LOITER_RAD + -- vehicle:set_guided_radius_and_direction(5, false) + follow_enabled = true + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled") + end + -- Don't know what to do with the 3rd switch position right now. + last_follow_active_state = active_state + end +end + +-- calculate difference between the target heading and the following vehicle heading +local function follow_target_angle(target_location) + + local follow_target_heading = follow:get_target_heading_deg() + + -- find the current location of the vehicle and calculate the bearing to its current target + local vehicle_location = ahrs:get_location() + local vehicle_heading = math.deg(vehicle_location:get_bearing(target_location)) + + local angle_target = follow_target_heading - vehicle_heading + 360 + if follow_target_heading > vehicle_heading then + angle_target = follow_target_heading - vehicle_heading + end + if angle_target > 180 then + angle_target = 360 - angle_target + end + + return angle_target +end + +-- main update function +local function update() + now = millis():tofloat() * 0.001 + current_location = ahrs:get_location() + + follow_check() + if not follow_active() then + return + end + + --[[ + get the current navigation target. Note that we must get this + before we check if we are in a follow to prevent a race condition + with vehicle:update_target_location() + --]] + local target_location -- = Location() of the target + local target_velocity -- = Vector3f() -- velocity of lead vehicle + local target_distance -- = Vector3f() -- vector to lead vehicle + local target_offsets -- = Vector3f() -- vector to lead vehicle + offsets + local target_distance_offsets -- vector to the target taking offsets into account + + local vehicle_airspeed = ahrs:airspeed_estimate() + local current_target = vehicle:get_target_location() + + -- just because of the methods available on AP_Follow, need to call these two methods + -- to get target_location, target_velocity, target distance and target + -- and yes target_offsets (hopefully the same value) is returned by both methods + -- even worse - both internally call get_target_location_and_Velocity, but making a single method + -- in AP_Follow is probably a high flash cost, so we just take the runtime hit + target_location, target_velocity = follow:get_target_location_and_velocity_ofs() + target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() + if target_location == nil or target_velocity == nil or target_distance_offsets == nil or current_target == nil then + lost_target_countdown = lost_target_countdown - 1 + if lost_target_countdown <= 0 then + if FOLL_FAIL_MODE:get() ~= nil then + vehicle:set_mode(FOLL_FAIL_MODE:get()) + end + follow_enabled = false + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target") + end + return + else + -- have a good target so reset the countdown + lost_target_countdown = LOST_TARGET_TIMEOUT + end + + -- default the desired airspeed to the value returned by AP_Follow (this mgith get overrride below) + -- local xy_dist = target_distance_offsets:length() + + xy_dist = current_location:get_distance(target_location) + if math.floor(now_distance) ~= math.floor(now) then + now_distance = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" xy_dist %f", xy_dist)) + end + + local target_airspeed = target_velocity:length() + local desired_airspeed = target_airspeed + local airspeed_difference = target_airspeed - vehicle_airspeed + if math.abs(airspeed_difference) > 15 then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": airspeed DIFFERENCE: " .. airspeed_difference ) + end + -- this is the projected distance to the target if we acheive the incremental airspeed increase targeted by AP_Follow + local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS + --projected_distance = xy_dist + airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS + + -- set the target frame as per user set parameter this might be a bad idea + --[[ + local altitude_frame = FOLL_ALT_TYPE:get() + if altitude_frame == nil then + altitude_frame = 0 + end + target_location:change_alt_frame(altitude_frame) + --]] + + -- next we try to match the airspeed of the target vehicle, calculating if we + -- need to speed up if too far behind, or slow down if too close + + -- if the current velocity will not catch up to the target then we need to speed up + -- use DISTANCE_LOOKAHEAD_SECONDS seconds as a reasonable time to try to catch up + -- first figure out how far away we will be from the required location in DISTANCE_LOOKAHEAD_SECONDS seconds if we maintain the current vehicle and target airspeeds + -- There is nothing magic about 12, it is just "what works" + local target_angle = follow_target_angle(target_location) + local target_heading = follow:get_target_heading_deg() + + local too_close = (xy_dist < target_airspeed * 5) and (target_angle < OVERSHOOT_ANGLE) + if too_close then + if math.floor(now_too_close) ~= math.floor(now) then + now_too_close = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f max target airspeed %f target angle %f", xy_dist, airspeed_difference, target_airspeed * DISTANCE_LOOKAHEAD_SECONDS / 1, target_angle)) + end + end + + -- we've overshot if the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS + -- local overshot = not too_close and (xy_dist < vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS) and target_angle > 45 + -- gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f target angle %f", xy_dist, target_airspeed, target_angle)) + local overshot = not too_close and projected_distance > 0 and target_angle >= OVERSHOOT_ANGLE + if overshot then + if math.floor(now_overshot) ~= math.floor(now) then + now_overshot = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("OVERSHOT xy_dist: %f airspeed difference: %f max distance %f target angle %f", xy_dist, vehicle_airspeed, vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS, target_angle)) + end + end + + if overshot or too_close or too_close_follow_up > 0 then + -- we have overshot or are just about to hit the target, so artificially jump it ahead 500m + -- note that is after setting xy_dist so that we still calculate airspeed below + -- according to the desired location not the one we are telling vehicle about + + local wp_loiter_rad = WP_LOITER_RAD:get() + if xy_dist < wp_loiter_rad then + target_heading = target_angle + end + + if overshot or too_close then + too_close_follow_up = too_close_follow_up - 1 + else + too_close_follow_up = 20 + -- try to reduce the amount we ajust the airspeed also + end + desired_airspeed = target_airspeed + -- now calculate what airspeed we will need to fly for a few seconds to catch up the projected distance + -- need to slow down dramatically + if overshot then + desired_airspeed = target_airspeed - (target_airspeed - AIRSPEED_MIN:get())/2 + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": OVERSHOT desired speed: " .. desired_airspeed ) + else + too_close = true + desired_airspeed = target_airspeed - (target_airspeed - AIRSPEED_MIN:get())/8 + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": TOO CLOSE desired speed: " .. desired_airspeed .. " difference: " .. airspeed_difference .. " distance: " .. math.abs(projected_distance - xy_dist)) + end + else + -- AP_Follow doesn't speed up enough if wa are a long way from the target + if airspeed_difference > 0 and projected_distance > xy_dist and projected_distance > airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS then + local airspeed_max = AIRSPEED_MAX:get() + desired_airspeed = target_airspeed + (airspeed_max - target_airspeed) * (projected_distance - xy_dist) /xy_dist * AIRSPEED_GAIN + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": LONG DISTANCE %f projected %f airspeed diff %f desired airspeed %f", xy_dist, projected_distance, airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS, desired_airspeed)) + else + desired_airspeed = target_airspeed + 1 + end + end + + if too_close or (xy_dist < target_airspeed and desired_airspeed > AIRSPEED_MIN:get()) then + if math.floor(now_too_close) ~= math.floor(now) then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": TOO CLOSE set vehicle heading: " .. target_heading ) + end + set_vehicle_heading({ heading = target_heading}) + set_vehicle_target_altitude({alt = target_location:alt() * 0.01}) + else + local old_location = ahrs:get_location() + local new_target_location = old_location:copy() + new_target_location:lat(target_location:lat()) + new_target_location:lng(target_location:lng()) + new_target_location:alt(target_location:alt()) + + --set_vehicle_heading({ type = MAV_HEADING_TYPE.NONE, heading = target_heading}) + set_vehicle_target_location({lat = target_location:lat(), lng = target_location:lng(), alt = target_location:alt() * 0.01}) + --set_vehicle_target_altitude({alt = target_location:alt() * 0.01}) + end + set_vehicle_speed({speed = desired_airspeed}) +end + +-- wrapper around update(). This calls update() at 20Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 1000 * REFRESH_RATE +end + +gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + +-- start running update loop +return protected_wrapper() + diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 72de9a5b7e4f4..8dd3f414458e7 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3494,19 +3494,38 @@ function precland:healthy() end -- desc follow = {} +-- true if we have a valid target location estimate +---@return boolean +function follow:have_target() end + +-- get the SYSID_THISMAV of the target +---@return uint8_t_ud +function follow:get_target_sysid() end +singleton AP_Follow method get_target_sysid uint8_t + +-- get horizontal distance to target (including offset) in meters (for reporting purposes) +---@return float +function follow:get_distance_to_target() end + -- desc ---@return number|nil function follow:get_target_heading_deg() end --- desc ----@return Location_ud|nil ----@return Vector3f_ud|nil +-- get target's estimated location and velocity (in NED) +---@return Location_ud|nil -- location +---@return Vector3f_ud|nil -- velocity +function follow:get_target_location_and_velocity() end + +-- get target's estimated location and velocity (in NED), with offsets added +---@return Location_ud|nil -- location +---@return Vector3f_ud|nil -- velocity function follow:get_target_location_and_velocity_ofs() end --- desc ----@return Location_ud|nil +-- desc get distance vector to target (in meters) and target's velocity all in NED frame ---@return Vector3f_ud|nil -function follow:get_target_location_and_velocity() end +---@return Vector3f_ud|nil +---@return Vector3f_ud|nil +function follow:get_target_dist_and_vel_ned() end -- desc ---@return uint32_t_ud diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 6c6d9ba5f8202..cbeba405895b3 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -738,9 +738,12 @@ include AP_Follow/AP_Follow.h singleton AP_Follow depends AP_FOLLOW_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI) singleton AP_Follow rename follow singleton AP_Follow method have_target boolean +singleton AP_Follow method get_target_sysid uint8_t +singleton AP_Follow method get_distance_to_target float singleton AP_Follow method get_last_update_ms uint32_t singleton AP_Follow method get_target_location_and_velocity boolean Location'Null Vector3f'Null singleton AP_Follow method get_target_location_and_velocity_ofs boolean Location'Null Vector3f'Null +singleton AP_Follow method get_target_dist_and_vel_ned boolean Vector3f'Null Vector3f'Null Vector3f'Null singleton AP_Follow method get_target_heading_deg boolean float'Null include AC_PrecLand/AC_PrecLand.h From ef7f8c2f260fadae55262034aa51b4e9d249f0a0 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 20 Jul 2024 19:21:10 -0600 Subject: [PATCH 04/20] ArduPlane: Changes to support Plane follow lua script --- ArduPlane/GCS_Mavlink.cpp | 15 +++++++++++---- ArduPlane/commands.cpp | 4 +++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 7c8d547091e35..ccfeecf874f5a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -865,9 +865,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com requested_position.alt += plane.home.alt; requested_position.relative_alt = 0; } - +//printf("do repositon requested alt %d\n", requested_position.alt); 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); @@ -969,8 +969,12 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); + // Default value = no heading track + if ( int(packet.param1) == HEADING_TYPE_DEFAULT) { + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + return MAV_RESULT_ACCEPTED; // course over ground - if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int + } else if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int plane.guided_state.target_heading_type = GUIDED_HEADING_COG; plane.prev_WP_loc = plane.current_loc; // normal vehicle heading @@ -984,10 +988,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl 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(); +//printf("set heading %f degrees %f rad accel limit: %f\n", packet.param2, new_target_heading, plane.guided_state.target_heading_accel_limit); return MAV_RESULT_ACCEPTED; } +#endif // OFFBOARD_GUIDED == ENABLED + } // anything else ... return MAV_RESULT_UNSUPPORTED; diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 7879defdd794d..eae01f64be18e 100644 --- a/ArduPlane/commands.cpp +++ b/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(); From 74c21bf09ed870e987a9bb7e44c63097fa4b15c9 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 23 Aug 2024 10:45:18 -0600 Subject: [PATCH 05/20] AP_Scripting: Scripted follow in Plane --- .../AP_Scripting/applets/modules/speedpi.lua | 156 +++++ .../AP_Scripting/applets/plane_follow.lua | 537 ++++++++++++------ libraries/AP_Scripting/docs/docs.lua | 11 + .../generator/description/bindings.desc | 3 + 4 files changed, 549 insertions(+), 158 deletions(-) create mode 100644 libraries/AP_Scripting/applets/modules/speedpi.lua diff --git a/libraries/AP_Scripting/applets/modules/speedpi.lua b/libraries/AP_Scripting/applets/modules/speedpi.lua new file mode 100644 index 0000000000000..f2c971958211c --- /dev/null +++ b/libraries/AP_Scripting/applets/modules/speedpi.lua @@ -0,0 +1,156 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + SpeedPI + A simple "PI" controller for airspeed. Copied from Andrew Tridgell's original + work on the ArduPilot Aerobatics Lua scripts. + + Usage: + 1. drop it in the scripts/modules directory + 2. include in your own script using + local speedpi = requre("speedpi.lua") + 3. create an instance - you may need to tune the + local speed_controller = speedpi.speed_controller(0.1, 0.1, 2.5, airspeed_min, airspeed_max) + 4. call it's update() from your update() with the current airspeed and airspeed error + local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed) + 5. Set the vehicle airspeed based on the airspeed_new value returned from speedpi + +--]] + +local SpeedPI = {} + +SpeedPI.SCRIPT_VERSION = "4.6.0-002" +SpeedPI.SCRIPT_NAME = "Speed PI Controller" +SpeedPI.SCRIPT_NAME_SHORT = "SpeedPI" + +-- constrain a value between limits +function SpeedPI.constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +function SpeedPI.PI_controller(kP,kI,iMax,min,max) + -- the new instance. You can put public variables inside this self + -- declaration if you want to + local self = {} + + -- private fields as locals + local _kP = kP or 0.0 + local _kI = kI or 0.0 + local _iMax = iMax + local _min = min + local _max = max + local _last_t = nil + local _I = 0 + local _P = 0 + local _total = 0 + local _counter = 0 + local _target = 0 + local _current = 0 + local nowPI = millis():tofloat() * 0.001 + + -- update the controller. + function self.update(target, current) + local now = millis():tofloat() * 0.001 + if not _last_t then + _last_t = now + end + local dt = now - _last_t + _last_t = now + local err = target - current + _counter = _counter + 1 + + local P = _kP * err + if ((_total < _max and _total > _min) or + (_total >= _max and err < 0) or + (_total <= _min and err > 0)) then + _I = _I + _kI * err * dt + end + if _iMax then + _I = SpeedPI.constrain(_I, -_iMax, iMax) + end + local I = _I + local ret = target + P + I + if math.floor(now) ~= math.floor(nowPI) then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target %f err %f P %f I %f ret %f", target,err, P, I, ret) ) + nowPI = millis():tofloat() * 0.001 + end + _target = target + _current = current + _P = P + + ret = SpeedPI.constrain(ret, _min, _max) + _total = ret + return ret + end + + -- reset integrator to an initial value + function self.reset(integrator) + _I = integrator + end + + function self.set_I(I) + _kI = I + end + + function self.set_P(P) + _kP = P + end + + function self.set_Imax(Imax) + _iMax = Imax + end + + -- log the controller internals + function self.log(name, add_total) + -- allow for an external addition to total + -- Targ = Current + error ( target airspeed ) + -- Curr = Current airspeed input to the controller + -- P = calculated Proportional component + -- I = calculated Integral component + -- Total = calculated new Airspeed + -- Add - passed in as 0 + logger.write(name,'Targ,Curr,P,I,Total,Add','ffffff',_target,_current,_P,_I,_total,add_total) + end + + -- return the instance + return self + end + +function SpeedPI.speed_controller(kP_param, kI_param, iMax, sMin, sMax) + local self = {} + local speedpi = SpeedPI.PI_controller(kP_param, kI_param, iMax, sMin, sMax) + + function self.update(spd_current, spd_error) + local adjustment = speedpi.update(spd_current + spd_error, spd_current) + speedpi.log("ZSPI", 0) -- Z = scripted, S = speed, PI = PI controller + return adjustment + end + + function self.reset() + speedpi.reset(0) + end + + return self +end + +gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", SpeedPI.SCRIPT_NAME, SpeedPI.SCRIPT_VERSION) ) + +return SpeedPI \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 13f7cda4084ac..be3ecdea67075 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -1,48 +1,73 @@ --[[ - support follow "mode" in plane. Theis will actually use GUIDED mode with - a scripting switch to allow guided to track the vehicle id in FOLL_SYSID + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + Follow in Plane + Support follow "mode" in plane. Theis will actually use GUIDED mode with + a scripting switch to allow guided to track the vehicle id in FOLL_SYSID + Uses the AP_Follow library so all of the existing FOLL_* parameters are used + as documented for Copter, + add 3 more for this script + FOLL_EXIT_MODE - the mode to switch to when follow is turned of using the switch + FOLL_FAIL_MODE - the mode to switch to --]] -SCRIPT_VERSION = "4.6.0-003" +SCRIPT_VERSION = "4.6.0-018" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" -REFRESH_RATE = 0.1 -- in seconds, so 10Hz +REFRESH_RATE = 0.05 -- in seconds, so 20Hz LOST_TARGET_TIMEOUT = 5 / REFRESH_RATE -- 5 seconds AIRSPEED_GAIN = 1.2 -OVERSHOOT_ANGLE = 45 +OVERSHOOT_ANGLE = 75.0 +TURNING_ANGLE = 60.0 local PARAM_TABLE_KEY = 83 local PARAM_TABLE_PREFIX = "FOLL_" +-- FOLL_ALT_TYPE and Mavlink FRAME use different values +ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3} + MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} -MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3} +MAV_FRAME = {GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10} MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } -MAV_HEADING_TYPE = { COG = 0, HEADING = 1} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points +MAV_HEADING_TYPE = { COG = 0, HEADING = 1, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points -local MODE_GUIDED = 15 -local MODE_RTL = 11 -local MODE_LOITER = 12 +FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +DISTANCE_LOOKAHEAD_SECONDS = 5 local current_location = ahrs:get_location() local now = millis():tofloat() * 0.001 -local now_too_close = millis():tofloat() * 0.001 -local now_overshot = millis():tofloat() * 0.001 -local now_distance = millis():tofloat() * 0.001 +local now_too_close = now +local now_overshot = now +local now_distance = now +local now_results = now +local now_airspeed = now +local now_lost_target = now local follow_enabled = false local too_close_follow_up = 0 local lost_target_countdown = LOST_TARGET_TIMEOUT -DISTANCE_LOOKAHEAD_SECONDS = 10 -- bind a parameter to a variable -function bind_param(name) +local function bind_param(name) return Parameter(name) end -- add a parameter and bind it to a variable -function bind_add_param(name, idx, default_value) +local function bind_add_param(name, idx, default_value) assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) return bind_param(PARAM_TABLE_PREFIX .. name) end @@ -57,6 +82,9 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add -- we need these existing FOLL_ parametrs FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE') +FOLL_SYSID = bind_param('FOLL_SYSID') +FOLL_OFS_Y = bind_param('FOLL_OFS_Y') +FOLL_OFS_X = bind_param('FOLL_OFS_X') -- Add these FOLL_ parameters specifically for this script --[[ @@ -65,7 +93,7 @@ FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE') // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). // @User: Standard --]] -FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, MODE_RTL) +FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, FLIGHT_MODE.RTL) --[[ // @Param: FOLL_EXIT_MODE @@ -73,7 +101,7 @@ FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, MODE_RTL) // @Description: Mode to switch to when follow mode is exited normally // @User: Standard --]] -FOLL_EXIT_MODE = bind_add_param('EXIT_MODE', 8, MODE_LOITER) +FOLL_EXIT_MODE = bind_add_param('EXIT_MODE', 8, FLIGHT_MODE.LOITER) --[[ // @Param: FOLL_ACT_FN @@ -83,114 +111,206 @@ FOLL_EXIT_MODE = bind_add_param('EXIT_MODE', 8, MODE_LOITER) --]] FOLL_ACT_FN = bind_add_param("ACT_FN", 9, 301) +AIRSPEED_MIN = bind_param('AIRSPEED_MIN') +AIRSPEED_MAX = bind_param('AIRSPEED_MAX') +AIRSPEED_CRUISE = bind_param('AIRSPEED_CRUISE') +WP_LOITER_RAD = bind_param('WP_LOITER_RAD') + +local airspeed_max = AIRSPEED_MAX:get() or 25.0 +local airspeed_min = AIRSPEED_MIN:get() or 12.0 +local airspeed_cruise = AIRSPEED_CRUISE:get() or 18.0 + --[[ create a NaN value --]] local function NaN() return 0/0 end - -local now_altitude = millis():tofloat() * 0.001 -local function set_vehicle_target_altitude(target) - if math.floor(now) ~= math.floor(now_altitude) then - now_altitude = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": alt: " .. target.alt) +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin end - - if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { frame = MAV_FRAME.GLOBAL, - z = target.alt }) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") + if v > vmax then + v = vmax end + return v end +local speedpi = require("speedpi") +local speed_controller = speedpi.speed_controller(0.1, 0.1, 2.0, airspeed_min, airspeed_max) -local function set_vehicle_target_location(target) +local function follow_frame_to_mavlink(follow_frame) + local mavlink_frame = MAV_FRAME.GLOBAL; + if (follow_frame == ALT_FRAME.TERRAIN) then + mavlink_frame = MAV_FRAME.GLOBAL_TERRAIN_ALT + end + if (follow_frame == ALT_FRAME.RELATIVE) then + mavlink_frame = MAV_FRAME.GLOBAL_RELATIVE_ALT + end + return mavlink_frame +end + +local now_altitude = millis():tofloat() * 0.001 +-- target.alt = new target altitude in meters +-- set_vehicle_target_altitude() Parameters +-- target.frame = Altitude frame MAV_FRAME, it's very important to get this right! +-- target.alt = altitude in meters to acheive +-- target.accel = z acceleration to altitude (1000.0 = max) +local function set_vehicle_target_altitude(target) + local home_location = ahrs:get_home() + local acceleration = target.accel or 1000.0 -- default to maximum z acceleration if math.floor(now) ~= math.floor(now_altitude) then now_altitude = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. " location : alt: " .. target.alt) + --[[ + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": set heading alt: %.1f", target.alt) ) + if home_location ~= nil then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(" set location : alt home relative: %.1f", (target.alt - (home_location:alt() * 0.01)))) + end + --]] end - - if not gcs:run_command_int(MAV_CMD_INT.DO_REPOSITION, { frame = MAV_FRAME.GLOBAL, - p1 = target.groundspeed or -1, - p4 = target.yaw or NaN(), - x = target.lat, - y = target.lng, - z = target.alt }) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_REPOSITION returned false") + if target.alt == nil then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude") + return end - if target.alt > 0 then - set_vehicle_target_altitude({alt = target.alt}) + -- GUIDED_CHANGE_ALTITUDE takes altitude in meters + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { + frame = follow_frame_to_mavlink(target.frame), + p3 = acceleration, + z = target.alt }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") end end local now_heading = millis():tofloat() * 0.001 +-- set_vehicle_heading() Parameters +-- heading.heading_type = MAV_HEADING_TYPE (defaults to HEADING) +-- heading.heading = the target heading in degrees +-- heading.accel = rate/acceleration to acheive the heading 0 = max local function set_vehicle_heading(heading) local heading_type = heading.type or MAV_HEADING_TYPE.HEADING + local heading_heading = heading.heading or 0 + local heading_accel = heading.accel or 0.0 - if math.floor(now) ~= math.floor(now_heading) then + if heading_type ~= MAV_HEADING_TYPE.DEFAULT and math.floor(now) ~= math.floor(now_heading) then now_heading = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": heading: " .. heading.heading) end if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL, - p1 = heading_type, p2 = heading.heading }) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_HEADING returned false") + p1 = heading_type, + p2 = heading_heading, + p3 = heading_accel }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_HEADING failed") end end +-- set_vehicle_speed() Parameters +-- speed.speed - new target speed +-- speed.type - speed type MAV_SPEED_TYPE +-- speed.throttle - new target throttle (used if speed = 0) +-- speed.slew - specified slew rate to hit the target speed, rather than jumping to it immediately 0 = maximum acceleration local function set_vehicle_speed(speed) local new_speed = speed.speed or 0.0 local speed_type = speed.type or MAV_SPEED_TYPE.AIRSPEED local throttle = speed.throttle or 0.0 local slew = speed.slew or 0.0 local mode = vehicle:get_mode() - if speed_type == MAV_SPEED_TYPE.AIRSPEED and speed.slew ~= 0 and mode == MODE_GUIDED then - if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, - p1 = speed_type, p2 = new_speed, p3 = slew }) then - -- if not gcs:command_int(MAV_FRAME.GLOBAL, MAV_CMD_INT.GUIDED_CHANGE_SPEED, { speed_type , new_speed, slew, 0, 0, 0, 0} ) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_SPEED returned false") + if speed_type == MAV_SPEED_TYPE.AIRSPEED and mode == FLIGHT_MODE.GUIDED then + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = slew }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_SPEED failed") end else if not gcs:run_command_int(MAV_CMD_INT.DO_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, - p1 = speed_type, p2 = new_speed, p3 = throttle }) then - -- if not gcs:command_int(MAV_FRAME.GLOBAL, MAV_CMD_INT.DO_CHANGE_SPEED, { speed.type or MAV_SPEED_TYPE.AIRSPEED , new_speed, throttle, 0, 0, 0, 0} ) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_CHANGE_SPEED returned false") + p1 = speed_type, + p2 = new_speed, + p3 = throttle }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_CHANGE_SPEED failed") + end + end +end + +-- set_vehicle_target_location() Parameters +-- target.groundspeed (-1 for ignore) +-- target.radius (defaults to 2m) +-- target.yaw - not really yaw - it's the loiter direction -1 = CCW, 1 = CW NaN = default +-- target.lat - latitude in decimal degrees +-- target.lng - longitude in decimal degrees +-- target.alt - target alitude in meters +local function set_vehicle_target_location(target) + local home_location = ahrs:get_home() + local radius = target.radius or 2.0 + local angle = target.turning_angle or 0 + local yaw = target.yaw or 1 + -- If we are on the right side of the vehicle make sure any loitering is CCW (moves away from the other plane) + if FOLL_OFS_Y:get() > 0 or angle < 0 then + yaw = -yaw + end + + if math.floor(now) ~= math.floor(now_altitude) then + now_altitude = millis():tofloat() * 0.001 + if home_location ~= nil then + --gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(" set location : alt home relative: %.1f", (target.alt - (home_location:alt() * 0.01)))) end + --gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(" set location : alt: %.1f", target.alt)) + end + + -- if we were in HEADING mode, need to switch out of it so that REPOSITION will work + -- Note that MAVLink DO_REPOSITION requires altitude in meters + set_vehicle_heading({type = MAV_HEADING_TYPE.DEFAULT}) + if not gcs:run_command_int(MAV_CMD_INT.DO_REPOSITION, { frame = follow_frame_to_mavlink(target.frame), + p1 = target.groundspeed or -1, + p2 = 1, + p3 = radius, + p4 = yaw, + x = target.lat, + y = target.lng, + z = target.alt }) then -- altitude in m + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_REPOSITION returned false") end end local last_follow_active_state = rc:get_aux_cached(FOLL_ACT_FN:get()) -AIRSPEED_MIN = bind_param('AIRSPEED_MIN') -AIRSPEED_MAX = bind_param('AIRSPEED_MAX') -AIRSPEED_CRUISE = bind_param('AIRSPEED_CRUISE') -FOLL_OFS_Y = bind_param('FOLL_OFS_Y') -WP_LOITER_RAD = bind_param('WP_LOITER_RAD') --[[ return true if we are in a state where follow can apply --]] +local reported_target = true local function follow_active() local mode = vehicle:get_mode() - if mode == MODE_GUIDED then + if mode == FLIGHT_MODE.GUIDED then if follow_enabled then if follow:have_target() then - return true + reported_target = true else - return false + if reported_target then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid()) + end + reported_target = false end end + else + reported_target = false end - return false + return reported_target end --[[ check for user activating follow using an RC switch set HIGH --]] local function follow_check() - local active_state = rc:get_aux_cached(FOLL_ACT_FN:get()) + if FOLL_ACT_FN == nil then + return + end + local foll_act_fn = FOLL_ACT_FN:get() + if foll_act_fn == nil then + return + end + local active_state = rc:get_aux_cached(foll_act_fn) if (active_state ~= last_follow_active_state) then if( active_state == 0) then if follow_enabled then @@ -204,9 +324,8 @@ local function follow_check() gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": must be armed") end -- Follow enabled - switch to guided mode - vehicle:set_mode(MODE_GUIDED) + vehicle:set_mode(FLIGHT_MODE.GUIDED) -- Force Guided Mode to not loiter by setting a tiny loiter radius. Otherwise Guide Mode will try loiter around the target vehichle when it gets inside WP_LOITER_RAD - -- vehicle:set_guided_radius_and_direction(5, false) follow_enabled = true gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled") end @@ -215,22 +334,33 @@ local function follow_check() end end --- calculate difference between the target heading and the following vehicle heading -local function follow_target_angle(target_location) +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end - local follow_target_heading = follow:get_target_heading_deg() +local function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +-- calculate difference between the target heading and the following vehicle heading +local function follow_target_angle(target_heading, target_location) -- find the current location of the vehicle and calculate the bearing to its current target - local vehicle_location = ahrs:get_location() - local vehicle_heading = math.deg(vehicle_location:get_bearing(target_location)) + local vehicle_heading = math.deg(current_location:get_bearing(target_location)) - local angle_target = follow_target_heading - vehicle_heading + 360 - if follow_target_heading > vehicle_heading then - angle_target = follow_target_heading - vehicle_heading - end - if angle_target > 180 then - angle_target = 360 - angle_target + local angle_target = target_heading - vehicle_heading + 360 + if target_heading > vehicle_heading then + angle_target = target_heading - vehicle_heading end + angle_target = wrap_180(angle_target) return angle_target end @@ -245,69 +375,75 @@ local function update() return end + -- set the target frame as per user set parameter - this is fundamental to this working correctly + local altitude_frame = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL + local wp_loiter_rad = WP_LOITER_RAD:get() + local close_distance = airspeed_cruise * 2.0 + --[[ - get the current navigation target. Note that we must get this - before we check if we are in a follow to prevent a race condition - with vehicle:update_target_location() + get the current navigation target. --]] local target_location -- = Location() of the target + local target_location_offset local target_velocity -- = Vector3f() -- velocity of lead vehicle + --local target_velocity_offset -- = Vector3f() -- velocity of lead vehicle local target_distance -- = Vector3f() -- vector to lead vehicle - local target_offsets -- = Vector3f() -- vector to lead vehicle + offsets + --local target_offsets -- = Vector3f() -- vector to lead vehicle + offsets local target_distance_offsets -- vector to the target taking offsets into account local vehicle_airspeed = ahrs:airspeed_estimate() local current_target = vehicle:get_target_location() - -- just because of the methods available on AP_Follow, need to call these two methods + -- because of the methods available on AP_Follow, need to call these two methods -- to get target_location, target_velocity, target distance and target -- and yes target_offsets (hopefully the same value) is returned by both methods -- even worse - both internally call get_target_location_and_Velocity, but making a single method -- in AP_Follow is probably a high flash cost, so we just take the runtime hit - target_location, target_velocity = follow:get_target_location_and_velocity_ofs() - target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() + target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() -- this has to be first + 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() if target_location == nil or target_velocity == nil or target_distance_offsets == nil or current_target == nil then lost_target_countdown = lost_target_countdown - 1 if lost_target_countdown <= 0 then if FOLL_FAIL_MODE:get() ~= nil then vehicle:set_mode(FOLL_FAIL_MODE:get()) + else + vehicle:set_mode(FLIGHT_MODE.RTL) end follow_enabled = false - gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target") + return + end + if math.floor(now) ~= math.floor(now_lost_target) then + now_lost_target = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target: " .. FOLL_SYSID:get()) end + -- if we have temporarily lost the target, maintain the heading of the target while we wait for LOST_TARGET_TIMEOUT seconds to re-aquire it + set_vehicle_heading({heading = target_heading}) return else -- have a good target so reset the countdown lost_target_countdown = LOST_TARGET_TIMEOUT end - - -- default the desired airspeed to the value returned by AP_Follow (this mgith get overrride below) - -- local xy_dist = target_distance_offsets:length() - - xy_dist = current_location:get_distance(target_location) - if math.floor(now_distance) ~= math.floor(now) then - now_distance = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" xy_dist %f", xy_dist)) - end + + local target_angle = follow_target_angle(target_heading, target_location) + local offset_angle = follow_target_angle(target_heading, target_location_offset) + local desired_heading = target_heading local target_airspeed = target_velocity:length() local desired_airspeed = target_airspeed - local airspeed_difference = target_airspeed - vehicle_airspeed - if math.abs(airspeed_difference) > 15 then - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": airspeed DIFFERENCE: " .. airspeed_difference ) + local airspeed_difference = vehicle_airspeed - target_airspeed + + -- distance seem to be out by about 1s at approximately current airspeed just eyeballing it. + xy_dist = math.abs(xy_dist) - vehicle_airspeed * 0.92 + -- xy_dist will always be a positive value. To get -v to represent overshoot, use the offset_angle + -- to decide if the target is behind. + if (math.abs(xy_dist) < wp_loiter_rad) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then + xy_dist = -xy_dist end - -- this is the projected distance to the target if we acheive the incremental airspeed increase targeted by AP_Follow + local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS - --projected_distance = xy_dist + airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS - - -- set the target frame as per user set parameter this might be a bad idea - --[[ - local altitude_frame = FOLL_ALT_TYPE:get() - if altitude_frame == nil then - altitude_frame = 0 - end - target_location:change_alt_frame(altitude_frame) - --]] -- next we try to match the airspeed of the target vehicle, calculating if we -- need to speed up if too far behind, or slow down if too close @@ -316,84 +452,169 @@ local function update() -- use DISTANCE_LOOKAHEAD_SECONDS seconds as a reasonable time to try to catch up -- first figure out how far away we will be from the required location in DISTANCE_LOOKAHEAD_SECONDS seconds if we maintain the current vehicle and target airspeeds -- There is nothing magic about 12, it is just "what works" - local target_angle = follow_target_angle(target_location) - local target_heading = follow:get_target_heading_deg() - - local too_close = (xy_dist < target_airspeed * 5) and (target_angle < OVERSHOOT_ANGLE) + + -- don't count it as close if the heading is diverging (i.e. the target plane is turning) + --local too_close = (xy_dist > 0) and (math.abs(xy_dist) < close_distance) and (offset_angle < OVERSHOOT_ANGLE) + local too_close = (projected_distance > 0) and (math.abs(projected_distance) < close_distance) and (offset_angle < OVERSHOOT_ANGLE) if too_close then if math.floor(now_too_close) ~= math.floor(now) then now_too_close = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f max target airspeed %f target angle %f", xy_dist, airspeed_difference, target_airspeed * DISTANCE_LOOKAHEAD_SECONDS / 1, target_angle)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT ) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format("TOO CLOSE: xy_dist: %.0f close dist %.0f target angle %.0f offset %.0f", xy_dist, close_distance, target_angle, offset_angle)) end end - -- we've overshot if the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS - -- local overshot = not too_close and (xy_dist < vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS) and target_angle > 45 - -- gcs:send_text(MAV_SEVERITY.NOTICE, string.format("TOO CLOSE xy_dist: %f target arspeed: %f target angle %f", xy_dist, target_airspeed, target_angle)) - local overshot = not too_close and projected_distance > 0 and target_angle >= OVERSHOOT_ANGLE + -- we've overshot if + -- the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS (projected_distance) + -- or the distance to the target location is already negative AND + -- the target is very close OR the angle to the target plane is effectively backwards + local overshot = not too_close and (projected_distance < 0 or xy_dist < 0) and (math.abs(xy_dist) < airspeed_max * 2.0) if overshot then if math.floor(now_overshot) ~= math.floor(now) then now_overshot = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.NOTICE, string.format("OVERSHOT xy_dist: %f airspeed difference: %f max distance %f target angle %f", xy_dist, vehicle_airspeed, vehicle_airspeed * DISTANCE_LOOKAHEAD_SECONDS, target_angle)) + if projected_distance < 0 or xy_dist < 0 then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT reason xy_dist: %.0f projected: %.1f", xy_dist, projected_distance)) + end + if target_angle >= OVERSHOOT_ANGLE then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT reason angle: %.0f overshoot: %.1f", target_angle, OVERSHOOT_ANGLE)) + end + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT reason distance: %.0f project %0.f close: %.0f", xy_dist, projected_distance, close_distance * 2)) end end - + -- xy 25 proj 20 = 5 + -- xy -25 proj -20 = 5 + -- xy -5 proj 5 = 10 + -- xy 5 proj -5 = 10 + -- xy 23 proj 34 = 10 + local distance = math.abs(xy_dist - projected_distance) + local distance_error = constrain(math.abs(projected_distance) / (close_distance * DISTANCE_LOOKAHEAD_SECONDS), 0.0, 1.0) if overshot or too_close or too_close_follow_up > 0 then - -- we have overshot or are just about to hit the target, so artificially jump it ahead 500m - -- note that is after setting xy_dist so that we still calculate airspeed below - -- according to the desired location not the one we are telling vehicle about - - local wp_loiter_rad = WP_LOITER_RAD:get() - if xy_dist < wp_loiter_rad then - target_heading = target_angle - end + -- special cases if we have overshot or are just about to hit the target, + -- if we are too close the target_angle will likely be wrong + desired_heading = target_heading - if overshot or too_close then - too_close_follow_up = too_close_follow_up - 1 - else - too_close_follow_up = 20 - -- try to reduce the amount we ajust the airspeed also + if too_close_follow_up > 0 then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOO CLOSE follow up: %d", too_close_follow_up) ) end - desired_airspeed = target_airspeed -- now calculate what airspeed we will need to fly for a few seconds to catch up the projected distance - -- need to slow down dramatically + -- need to slow down dramatically. First we try to match the TARGET airspeed unless special cases kick in if overshot then - desired_airspeed = target_airspeed - (target_airspeed - AIRSPEED_MIN:get())/2 - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": OVERSHOT desired speed: " .. desired_airspeed ) + -- if we aren't going fast enough speed up, otherwise stay the course + + --if math.abs(projected_distance) < X_FUDGE then -- ignore it, close enough + -- desired_airspeed = target_airspeed + --else + if projected_distance < 0 then -- going too fast - slow down + desired_airspeed = vehicle_airspeed - (((vehicle_airspeed - airspeed_min) * distance_error) / 2.0) + else -- project that we are going too slow - speed up a bit but not too much + desired_airspeed = vehicle_airspeed - (((airspeed_max - vehicle_airspeed) * distance_error) / 8.0) + end + if math.floor(now_airspeed) ~= math.floor(now) then + now_airspeed = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT airspeed projected %.0f desired speed: %.1f error: %.3f", projected_distance, desired_airspeed, distance_error) ) + end + else too_close = true - desired_airspeed = target_airspeed - (target_airspeed - AIRSPEED_MIN:get())/8 - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": TOO CLOSE desired speed: " .. desired_airspeed .. " difference: " .. airspeed_difference .. " distance: " .. math.abs(projected_distance - xy_dist)) + -- slow down so we don't overshoot + if -xy_dist < -projected_distance or xy_dist < 0 or projected_distance < 0 then -- slow down a bit more than, we are going too fast + desired_airspeed = vehicle_airspeed - (((vehicle_airspeed - airspeed_min) * distance_error) / 4.0) + else + desired_airspeed = vehicle_airspeed + (((vehicle_airspeed - airspeed_min) * distance_error) / 8.0) + end + if math.floor(now_airspeed) ~= math.floor(now) then + now_airspeed = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOOCLOSE dst %.0f prj %0.f asp new: %.1f err: %.3f", xy_dist, projected_distance, desired_airspeed, distance_error) ) + end + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOO CLOSE desired speed: %.1f", desired_airspeed) ) + end + if too_close_follow_up > 0 then + too_close_follow_up = too_close_follow_up - 1 + else + too_close_follow_up = 10 + end + if math.floor(now_distance) ~= math.floor(now) then + local where = "CLOSE" + if not too_close then + where = "OVERSHOT" + end + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": %s distance %.0f projected %.0f ", where, xy_dist, projected_distance)) + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": %s target %.1f desired airspeed %.1f", where, target_airspeed, desired_airspeed)) + now_distance = millis():tofloat() * 0.001 end else + too_close_follow_up = 0 -- AP_Follow doesn't speed up enough if wa are a long way from the target - if airspeed_difference > 0 and projected_distance > xy_dist and projected_distance > airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS then - local airspeed_max = AIRSPEED_MAX:get() - desired_airspeed = target_airspeed + (airspeed_max - target_airspeed) * (projected_distance - xy_dist) /xy_dist * AIRSPEED_GAIN - gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(": LONG DISTANCE %f projected %f airspeed diff %f desired airspeed %f", xy_dist, projected_distance, airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS, desired_airspeed)) + -- what we want is a. to figure out if we are a long way from the target. Basically if our current airspeed will not catch up in DISTANCE_LOOKAHEAD_SECONDS + -- be calculate an increasing target speed up to AIRSPEED_MAX based on the projected distance from the target. + if projected_distance > 0 then + local incremental_speed = projected_distance / DISTANCE_LOOKAHEAD_SECONDS + desired_airspeed = constrain(target_airspeed + incremental_speed, airspeed_min, airspeed_max) + -- desired_airspeed = target_airspeed + (airspeed_max - target_airspeed) * (projected_distance - xy_dist) /xy_dist * AIRSPEED_GAIN + -- gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": LONG LONG projected %.0f target %.1f desired %.1f", projected_distance, target_airspeed, desired_airspeed)) else - desired_airspeed = target_airspeed + 1 + desired_airspeed = target_airspeed + 2 + end + --distance_error = constrain(math.abs((-xy_dist + projected_distance)) / airspeed_cruise * DISTANCE_LOOKAHEAD_SECONDS / 2.0, 0.0, 1.0) + desired_airspeed = vehicle_airspeed + ((airspeed_max - vehicle_airspeed) * distance_error) + if math.floor(now_distance) ~= math.floor(now) then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG distance %.0f projected %.0f close %.0f", xy_dist, projected_distance, close_distance)) + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG target %.1f desired airspeed %.1f", target_airspeed, desired_airspeed)) + now_distance = millis():tofloat() * 0.001 end end - if too_close or (xy_dist < target_airspeed and desired_airspeed > AIRSPEED_MIN:get()) then - if math.floor(now_too_close) ~= math.floor(now) then - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. ": TOO CLOSE set vehicle heading: " .. target_heading ) - end - set_vehicle_heading({ heading = target_heading}) - set_vehicle_target_altitude({alt = target_location:alt() * 0.01}) + local old_location = ahrs:get_location() + local current_altitude = 0.0 + if old_location ~= nil then + current_altitude = old_location:alt() * 0.01 + end + + local new_target_location = old_location:copy() + local target_altitude = 0.0 + + target_altitude = target_location_offset:alt() * 0.01 + new_target_location:lat(target_location_offset:lat()) + new_target_location:lng(target_location_offset:lng()) + new_target_location:alt(target_location_offset:alt()) -- location uses cm for altitude + -- if the target is turning or we are a fair way away from it then we use location, otherwise we use heading + if (math.abs(xy_dist) <= wp_loiter_rad and math.abs(target_angle) < TURNING_ANGLE) + or math.abs(xy_dist) > close_distance then + set_vehicle_target_location({lat = target_location_offset:lat(), + lng = target_location_offset:lng(), + alt = target_altitude, + frame = altitude_frame, + turning_angle = target_angle}) else - local old_location = ahrs:get_location() - local new_target_location = old_location:copy() - new_target_location:lat(target_location:lat()) - new_target_location:lng(target_location:lng()) - new_target_location:alt(target_location:alt()) - - --set_vehicle_heading({ type = MAV_HEADING_TYPE.NONE, heading = target_heading}) - set_vehicle_target_location({lat = target_location:lat(), lng = target_location:lng(), alt = target_location:alt() * 0.01}) - --set_vehicle_target_altitude({alt = target_location:alt() * 0.01}) + set_vehicle_heading({heading = desired_heading}) + set_vehicle_target_altitude({alt = target_altitude, frame = altitude_frame}) -- pass altitude in meters (location has it in cm) end - set_vehicle_speed({speed = desired_airspeed}) + local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed) + if math.floor(now_results) ~= math.floor(now) then + --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format("vehicle x %.1f y %.1f length %.1f", vehicle_vector:x(), vehicle_vector:y(), vehicle_vector:length())) + --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format("target x %.1f y %.1f length %.1f", target_vector:x(), target_vector:y(), target_vector:length())) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format("xy_dist: %.0f projected %.0f angle target %.1f offset %.1f", xy_dist, projected_distance, target_angle, offset_angle)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f diff %.0f err %.3f ", xy_dist, projected_distance, distance, distance_error)) + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f asp %.1f NEW %.1f ", xy_dist, projected_distance, desired_airspeed, airspeed_new)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": hdg:%.0f alt: %.0f ang off %.0f trn %.0f", target_heading, target_location:alt() * 0.01, offset_angle, target_angle )) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target alt: %.1f", target_location:alt() * 0.01 )) + if math.abs(xy_dist) < wp_loiter_rad and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" REVERSE: distance %.1f offset_angle %.1f ", xy_dist, offset_angle )) + end + now_results = millis():tofloat() * 0.001 + end + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": distance %f speed %s heading:%f alt: %f", xy_dist, desired_airspeed, target_heading, target_location:alt() )) + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + + logger.write("ZPFL",'Dst,DstP,AspT,Asp,AspO,Hdg,Alt,AltT','ffffffff', + xy_dist, + projected_distance, + target_airspeed, + vehicle_airspeed, + desired_airspeed, + target_heading, + current_altitude, + target_altitude) end -- wrapper around update(). This calls update() at 20Hz, @@ -411,7 +632,7 @@ local function protected_wrapper() end gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) - + -- start running update loop return protected_wrapper() diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 8dd3f414458e7..21f67fcd529d1 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -938,6 +938,10 @@ function Vector3f_ud:rotate_xy(param1) end ---@return Vector2f_ud function Vector3f_ud:xy() end +-- Project a vector onto this vector +---@param vector Vector3f_ud +function Vector3f_ud:project(vector) end + -- desc ---@class (exact) Quaternion_ud ---@operator mul(Quaternion_ud): Quaternion_ud @@ -1153,6 +1157,13 @@ function Location_ud:offset(ofs_north, ofs_east) end ---@return number -- horizontal distance in meters function Location_ud:get_distance(loc) end +-- Given a Location this calculates the north and east distance between the two locations in meters. +---@param loc1 Location_ud -- 1st location +---@param loc2 Location_ud -- 1st location +---@return float -- proportion the current location is between loc1 and loc2 +function Location_ud:line_path_proportion(loc1, loc2) end + + -- desc ---@class (exact) AP_EFI_Backend_ud local AP_EFI_Backend_ud = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index cbeba405895b3..25c9b9d066f85 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -22,6 +22,7 @@ userdata Location method get_distance_NE Vector2f Location userdata Location method get_alt_frame uint8_t userdata Location method change_alt_frame boolean Location::AltFrame'enum Location::AltFrame::ABSOLUTE Location::AltFrame::ABOVE_TERRAIN userdata Location method copy Location +userdata Location method line_path_proportion float Location Location include AP_AHRS/AP_AHRS.h @@ -48,6 +49,7 @@ singleton AP_AHRS method get_relative_position_D_home void float'Ref singleton AP_AHRS method home_is_set boolean singleton AP_AHRS method healthy boolean singleton AP_AHRS method airspeed_estimate boolean float'Null +singleton AP_AHRS method airspeed_vector_true boolean Vector3f'Null singleton AP_AHRS method get_vibration Vector3f singleton AP_AHRS method earth_to_body Vector3f Vector3f singleton AP_AHRS method body_to_earth Vector3f Vector3f @@ -159,6 +161,7 @@ userdata Vector3f method copy Vector3f userdata Vector3f method xy Vector2f userdata Vector3f method rotate_xy void float'skip_check userdata Vector3f method angle float Vector3f +userdata Vector3f method project void Vector3f userdata Vector2f field x float'skip_check read write userdata Vector2f field y float'skip_check read write From 2d2c1870ea68a95ceecc9962195b497d6a58846c Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 23 Aug 2024 11:06:42 -0600 Subject: [PATCH 06/20] ArduPlane: Scripted follow in Plane --- ArduPlane/GCS_Mavlink.cpp | 3 +-- ArduPlane/mode.cpp | 2 +- ArduPlane/mode_guided.cpp | 9 ++++++--- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index ccfeecf874f5a..c287112a167c6 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -865,7 +865,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com requested_position.alt += plane.home.alt; requested_position.relative_alt = 0; } -//printf("do repositon requested alt %d\n", requested_position.alt); + plane.set_guided_WP(requested_position); // Loiter radius for planes. Positive radius in meters, direction is controlled by Yaw (param4) value, parsed above @@ -990,7 +990,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl plane.guided_state.target_heading = new_target_heading; 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(); -//printf("set heading %f degrees %f rad accel limit: %f\n", packet.param2, new_target_heading, plane.guided_state.target_heading_accel_limit); return MAV_RESULT_ACCEPTED; } #endif // OFFBOARD_GUIDED == ENABLED diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index ade2abb2a317f..c6cf936b937a7 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -163,7 +163,7 @@ bool Mode::is_vtol_man_throttle() const void Mode::update_target_altitude() { Location target_location; - +//printf("Mode: update_target_altitude:\n" ); if (plane.landing.is_flaring()) { // during a landing flare, use TECS_LAND_SINK as a target sink // rate, and ignores the target altitude diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index a97bedcd7dffb..da78766622b88 100644 --- a/ArduPlane/mode_guided.cpp +++ b/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 50 + 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); From 65594aeb3c644b9a5ab0ac732f4af2381118a51b Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 23 Aug 2024 11:07:17 -0600 Subject: [PATCH 07/20] AP_Scripting: Scripted follow in Plane --- .../AP_Scripting/applets/plane_follow.md | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 libraries/AP_Scripting/applets/plane_follow.md diff --git a/libraries/AP_Scripting/applets/plane_follow.md b/libraries/AP_Scripting/applets/plane_follow.md new file mode 100644 index 0000000000000..497e6bbf520da --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.md @@ -0,0 +1,48 @@ +# Plane Follow + +This script implements follow functionality for Plane. The plane must be +flying in Guided mode and will trigger on a scripting switch. + + +# Parameters + +The script adds the following parameters to control it's behaviour. It uses +the existing FOLL parameters that are used for the Copter FOLLOW mode. + +## TERR_BRK_ENABLE + +This must be set to 1 to enable the script. + +## TERR_BRK_ALT + +This is the terrain altitude threshold for engaging BRAKE mode. The +onboard terrain system must be enabled with TERRAIN_ENABLE=1 and +terrain must have either been preloaded to the vehicle (see +https://terrain.ardupilot.org ) or be available from the ground +station over MAVLink. + +Make sure you set sufficient margin to cope with obstacles such as +trees or any local towers or other obstacles. + +## FOLL_ALT_TYPE + +The existing FOLLOW mode parameter that specifies the target frame for +altitudes to be used when following. + +## FOLL_FAIL_MODE + +This is the mode the plane will change to if following fails. Failure happens +if the following plane loses telemetry from the target, or the distance exceeds +FOLL_???_DIST + +# Operation + +Install the lua script in the APM/SCRIPTS directory on the flight +controllers microSD card. Review the above parameter descriptions and +decide on the right parameter values for your vehicle and operations. + +Most of the follow logic is in AP_Follow which is part of the ArduPilot c++ +code, so this script just calls the existing methods to do things like +lookup the SYSID of the vehicle to follow and calculate the direction and distance +to the target, which should ideally be another fixed wing plane, or VTOL in +fixed wing mode. From 3d2a747142539cd890eefe5bd0d098da77331df3 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 23 Aug 2024 11:35:04 -0600 Subject: [PATCH 08/20] ArduPlane: Scripted follow in Plane --- ArduPlane/GCS_Mavlink.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c287112a167c6..07e3ac3977b29 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -970,19 +970,26 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); // Default value = no heading track + /* requires ardupilot_mega.xml change if ( int(packet.param1) == HEADING_TYPE_DEFAULT) { plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; return MAV_RESULT_ACCEPTED; // course over ground - } else if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int + } else + */ + if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int 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 plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING; } else { + // fudge for now if we get an invalid result we assume it's "DEFAULT" + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + return MAV_RESULT_ACCEPTED; + // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). - return MAV_RESULT_DENIED; + // return MAV_RESULT_DENIED; } plane.g2.guidedHeading.reset_I(); @@ -992,8 +999,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl plane.guided_state.target_heading_time_ms = AP_HAL::millis(); return MAV_RESULT_ACCEPTED; } -#endif // OFFBOARD_GUIDED == ENABLED - } // anything else ... return MAV_RESULT_UNSUPPORTED; From 0a254f0836a3324a73044d2625caa9bb80d61ece Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 23 Aug 2024 11:48:31 -0600 Subject: [PATCH 09/20] ArduPlane: Scripted follow in Plane --- ArduPlane/mode.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index c6cf936b937a7..c82c065357221 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -163,7 +163,6 @@ bool Mode::is_vtol_man_throttle() const void Mode::update_target_altitude() { Location target_location; -//printf("Mode: update_target_altitude:\n" ); if (plane.landing.is_flaring()) { // during a landing flare, use TECS_LAND_SINK as a target sink // rate, and ignores the target altitude From 7dccafb9e61536f1d77a1e6feabcc8d7e8cd387c Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 5 Sep 2024 15:20:22 -0600 Subject: [PATCH 10/20] ArduPlane: Scripted follow in Plane --- ArduPlane/GCS_Mavlink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 07e3ac3977b29..c8822bf74dfec 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -858,7 +858,9 @@ 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) { From 321bb8180100725a11687151b936c40f9b8d9da6 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 13 Sep 2024 10:15:41 -0600 Subject: [PATCH 11/20] AP_Scripting: Scripted follow in Plane --- .../AP_Scripting/applets/plane_follow.lua | 15 ++++---- .../AP_Scripting/applets/plane_follow.md | 36 ++++++++++--------- 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index be3ecdea67075..48d3d95249370 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -22,12 +22,12 @@ FOLL_FAIL_MODE - the mode to switch to --]] -SCRIPT_VERSION = "4.6.0-018" +SCRIPT_VERSION = "4.6.0-021" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" REFRESH_RATE = 0.05 -- in seconds, so 20Hz -LOST_TARGET_TIMEOUT = 5 / REFRESH_RATE -- 5 seconds +LOST_TARGET_TIMEOUT = 10 / REFRESH_RATE -- 5 seconds AIRSPEED_GAIN = 1.2 OVERSHOOT_ANGLE = 75.0 TURNING_ANGLE = 60.0 @@ -93,7 +93,7 @@ FOLL_OFS_X = bind_param('FOLL_OFS_X') // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). // @User: Standard --]] -FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, FLIGHT_MODE.RTL) +FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, FLIGHT_MODE.LOITER) --[[ // @Param: FOLL_EXIT_MODE @@ -325,8 +325,8 @@ local function follow_check() end -- Follow enabled - switch to guided mode vehicle:set_mode(FLIGHT_MODE.GUIDED) - -- Force Guided Mode to not loiter by setting a tiny loiter radius. Otherwise Guide Mode will try loiter around the target vehichle when it gets inside WP_LOITER_RAD follow_enabled = true + lost_target_countdown = LOST_TARGET_TIMEOUT gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled") end -- Don't know what to do with the 3rd switch position right now. @@ -407,20 +407,21 @@ local function update() if target_location == nil or target_velocity == nil or target_distance_offsets == nil or current_target == nil then lost_target_countdown = lost_target_countdown - 1 if lost_target_countdown <= 0 then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") if FOLL_FAIL_MODE:get() ~= nil then vehicle:set_mode(FOLL_FAIL_MODE:get()) else - vehicle:set_mode(FLIGHT_MODE.RTL) + vehicle:set_mode(FLIGHT_MODE.LOITER) end follow_enabled = false return end if math.floor(now) ~= math.floor(now_lost_target) then now_lost_target = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target: " .. FOLL_SYSID:get()) + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target: " .. FOLL_SYSID:get() .. " count: " .. lost_target_countdown) end -- if we have temporarily lost the target, maintain the heading of the target while we wait for LOST_TARGET_TIMEOUT seconds to re-aquire it - set_vehicle_heading({heading = target_heading}) + -- set_vehicle_heading({heading = target_heading}) return else -- have a good target so reset the countdown diff --git a/libraries/AP_Scripting/applets/plane_follow.md b/libraries/AP_Scripting/applets/plane_follow.md index 497e6bbf520da..d89b52ed5e37c 100644 --- a/libraries/AP_Scripting/applets/plane_follow.md +++ b/libraries/AP_Scripting/applets/plane_follow.md @@ -1,7 +1,10 @@ # Plane Follow This script implements follow functionality for Plane. The plane must be -flying in Guided mode and will trigger on a scripting switch. +flying in Guided mode and will trigger on a scripting switch. The target plane +must be connected via MAVLink and sending mavlink updates to the chase plane +running this script. The MAVLINK_SYSID of the target must be set in FOLL_SYSID, +and must be different from the MAVLINK_SYSID of the following plane. # Parameters @@ -9,31 +12,26 @@ flying in Guided mode and will trigger on a scripting switch. The script adds the following parameters to control it's behaviour. It uses the existing FOLL parameters that are used for the Copter FOLLOW mode. -## TERR_BRK_ENABLE - -This must be set to 1 to enable the script. +## FOLL_FAIL_MODE -## TERR_BRK_ALT +This is the mode the plane will change to if following fails. Failure happens +if the following plane loses telemetry from the target, or the distance exceeds +FOLL_DIST_MAX. -This is the terrain altitude threshold for engaging BRAKE mode. The -onboard terrain system must be enabled with TERRAIN_ENABLE=1 and -terrain must have either been preloaded to the vehicle (see -https://terrain.ardupilot.org ) or be available from the ground -station over MAVLink. +## FOLL_EXIT_MODE -Make sure you set sufficient margin to cope with obstacles such as -trees or any local towers or other obstacles. +The flight mode the plane will switch to if it exits following. ## FOLL_ALT_TYPE The existing FOLLOW mode parameter that specifies the target frame for altitudes to be used when following. -## FOLL_FAIL_MODE +## FOLL_ACT_FN -This is the mode the plane will change to if following fails. Failure happens -if the following plane loses telemetry from the target, or the distance exceeds -FOLL_???_DIST +The scripting action that will trigger the plane to start following. When this +happens the plane will switch to GUIDED mode and the script will use guided mode +commands to steer the plane towards the target. # Operation @@ -46,3 +44,9 @@ code, so this script just calls the existing methods to do things like lookup the SYSID of the vehicle to follow and calculate the direction and distance to the target, which should ideally be another fixed wing plane, or VTOL in fixed wing mode. + +The target location the plane will attempt to acheive will be offset from the target +vehicle location by FOLL_OFS_X and FOLL_OFS_Y. FOLL_OFS_Z will be offset against the +target vehicle, but also FOLL_ALT_TYPE will determine the altitude frame that the vehicle +will use when calculating the target altitude. See the definitions of these +parameters to understand how they work. From 09f99cdaaa86f6b4bca69e35a9d4e0f98676459e Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 14 Sep 2024 11:50:03 -0600 Subject: [PATCH 12/20] ArduPlane: Scripted follow in Plane --- libraries/AP_Follow/AP_Follow.cpp | 32 +++++++++++++++++++++++++++++++ libraries/AP_Follow/AP_Follow.h | 10 ++++++++++ 2 files changed, 42 insertions(+) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index b4acb2fc0499a..44b95cf19ef42 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -254,6 +254,9 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w return true; } + + + // get target's heading in degrees (0 = north, 90 = east) bool AP_Follow::get_target_heading_deg(float &heading) const { @@ -565,6 +568,35 @@ bool AP_Follow::have_target(void) const return true; } +// 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() -- THIS HAS TO BE FIRST + 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, + float &target_heading_ofs_deg + ) +{ + 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; + target_heading_ofs_deg = _bearing_to_target; + return true; +} + namespace AP { AP_Follow &follow() diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 48c43fc9ff470..cabcd735256f5 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -112,6 +112,16 @@ class AP_Follow // returns true if a follow option enabled bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; } + // + // Lua binding combo function(s) + // + // 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, + float &target_heading_ofs_deg + ); // parameter list static const struct AP_Param::GroupInfo var_info[]; From de04ee1fe3fce4ec09fb3bf447e82c9b132381e3 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 14 Sep 2024 12:41:11 -0600 Subject: [PATCH 13/20] AP_Scripting: Scripted follow in Plane --- .../AP_Scripting/applets/plane_follow.lua | 249 ++++++++++++++---- libraries/AP_Scripting/docs/docs.lua | 17 +- .../generator/description/bindings.desc | 2 + 3 files changed, 206 insertions(+), 62 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 48d3d95249370..53552a7ec628d 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -18,23 +18,17 @@ a scripting switch to allow guided to track the vehicle id in FOLL_SYSID Uses the AP_Follow library so all of the existing FOLL_* parameters are used as documented for Copter, + add 3 more for this script - FOLL_EXIT_MODE - the mode to switch to when follow is turned of using the switch - FOLL_FAIL_MODE - the mode to switch to + ZPF_EXIT_MODE - the mode to switch to when follow is turned of using the switch + ZPF_FAIL_MODE - the mode to switch to if the target is lost + ZPF_TIMEOUT - number of seconds to try to reaquire the target after losing it before failing + ZPF_OVRSHT_DEG - if the target is more than this many degrees left or right, assume an overshoot + ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.6.0-021" +SCRIPT_VERSION = "4.6.0-022" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" -REFRESH_RATE = 0.05 -- in seconds, so 20Hz -LOST_TARGET_TIMEOUT = 10 / REFRESH_RATE -- 5 seconds -AIRSPEED_GAIN = 1.2 -OVERSHOOT_ANGLE = 75.0 -TURNING_ANGLE = 60.0 - -local PARAM_TABLE_KEY = 83 -local PARAM_TABLE_PREFIX = "FOLL_" - -- FOLL_ALT_TYPE and Mavlink FRAME use different values ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3} @@ -57,15 +51,22 @@ local now_distance = now local now_results = now local now_airspeed = now local now_lost_target = now +local now_target_heading = now local follow_enabled = false local too_close_follow_up = 0 local lost_target_countdown = LOST_TARGET_TIMEOUT +local save_target_heading = {0.0, 0.0, 0.0} +local save_target_heading1 = -400.0 +local save_target_heading2 = -400.0 -- bind a parameter to a variable local function bind_param(name) return Parameter(name) end +local PARAM_TABLE_KEY = 100 +local PARAM_TABLE_PREFIX = "ZPF_" + -- add a parameter and bind it to a variable local function bind_add_param(name, idx, default_value) assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) @@ -78,7 +79,6 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add -- This uses the exisitng FOLL_* parameters and just adds a couple specific to this script -- but because most of the logic is already in AP_Follow (called by binding to follow:) there -- is no need to access them in the scriot --- FOLL_ -- we need these existing FOLL_ parametrs FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE') @@ -86,30 +86,71 @@ FOLL_SYSID = bind_param('FOLL_SYSID') FOLL_OFS_Y = bind_param('FOLL_OFS_Y') FOLL_OFS_X = bind_param('FOLL_OFS_X') --- Add these FOLL_ parameters specifically for this script +-- Add these ZPF_ parameters specifically for this script --[[ - // @Param: FOLL_FAIL_MODE + // @Param: ZPF_FAIL_MODE // @DisplayName: Plane Follow lost target mode // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). // @User: Standard --]] -FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, FLIGHT_MODE.LOITER) +ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.LOITER) --[[ - // @Param: FOLL_EXIT_MODE + // @Param: ZPF_EXIT_MODE // @DisplayName: Plane Follow exit mode // @Description: Mode to switch to when follow mode is exited normally // @User: Standard --]] -FOLL_EXIT_MODE = bind_add_param('EXIT_MODE', 8, FLIGHT_MODE.LOITER) +ZPF_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER) --[[ - // @Param: FOLL_ACT_FN + // @Param: ZPF_ACT_FN // @DisplayName: Plane Follow Scripting ActivationFunction // @Description: Setting an RC channel's _OPTION to this value will use it for Plane Follow enable/disable // @Range: 300 307 --]] -FOLL_ACT_FN = bind_add_param("ACT_FN", 9, 301) +ZPF_ACT_FN = bind_add_param("ACT_FN", 3, 301) + +--[[ + // @Param: ZPF_TIMEOUT + // @DisplayName: Plane Follow Scripting Timeout + // @Description: How long to try re-aquire a target if lost + // @Range: 0 30 + // @Units: seconds +--]] +ZPF_TIMEOUT = bind_add_param("TIMEOUT", 4, 19) + +--[[ + // @Param: ZPF_OVRSHT_DEG + // @DisplayName: Plane Follow Scripting Overshoot Angle + // @Description: If the target is greater than this many degrees left or right, assume an overshoot + // @Range: 0 180 + // @Units: degrees +--]] +ZPF_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75) + +--[[ + // @Param: ZPF_TURN_DEG + // @DisplayName: Plane Follow Scripting Turn Angle + // @Description: If the target is greater than this many degrees left or right, assume it's turning + // @Range: 0 180 + // @Units: degrees +--]] +ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 60) + +--[[ + // @Param: ZPF_DIST_CLOSE + // @DisplayName: Plane Follow Scripting Close Distance + // @Description: When closer than this distance assume we track by heading + // @Range: 0 100 + // @Units: meters +--]] +ZPF_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 40) + +REFRESH_RATE = 0.05 -- in seconds, so 20Hz +LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE +OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 +TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 AIRSPEED_MIN = bind_param('AIRSPEED_MIN') AIRSPEED_MAX = bind_param('AIRSPEED_MAX') @@ -137,7 +178,7 @@ local function constrain(v, vmin, vmax) end local speedpi = require("speedpi") -local speed_controller = speedpi.speed_controller(0.1, 0.1, 2.0, airspeed_min, airspeed_max) +local speed_controller = speedpi.speed_controller(0.1, 0.1, 5.0, airspeed_min, airspeed_max) local function follow_frame_to_mavlink(follow_frame) local mavlink_frame = MAV_FRAME.GLOBAL; @@ -272,7 +313,7 @@ local function set_vehicle_target_location(target) end end -local last_follow_active_state = rc:get_aux_cached(FOLL_ACT_FN:get()) +local last_follow_active_state = rc:get_aux_cached(ZPF_ACT_FN:get()) --[[ return true if we are in a state where follow can apply @@ -303,10 +344,10 @@ end check for user activating follow using an RC switch set HIGH --]] local function follow_check() - if FOLL_ACT_FN == nil then + if ZPF_ACT_FN == nil then return end - local foll_act_fn = FOLL_ACT_FN:get() + local foll_act_fn = ZPF_ACT_FN:get() if foll_act_fn == nil then return end @@ -315,7 +356,7 @@ local function follow_check() if( active_state == 0) then if follow_enabled then -- Follow disabled - return to EXIT mode - vehicle:set_mode(FOLL_EXIT_MODE:get()) + vehicle:set_mode(ZPF_EXIT_MODE:get()) follow_enabled = false gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled") end @@ -351,20 +392,23 @@ local function wrap_180(angle) end -- calculate difference between the target heading and the following vehicle heading -local function follow_target_angle(target_heading, target_location) +local function follow_target_angle(target_heading_follow, target_location_follow) -- find the current location of the vehicle and calculate the bearing to its current target - local vehicle_heading = math.deg(current_location:get_bearing(target_location)) + local follow_heading = math.deg(current_location:get_bearing(target_location_follow)) - local angle_target = target_heading - vehicle_heading + 360 - if target_heading > vehicle_heading then - angle_target = target_heading - vehicle_heading + local angle_target_return = wrap_180(target_heading_follow - follow_heading) + --[[ + if follow_heading > vehicle_heading then + angle_target_return = target_heading - vehicle_heading end - angle_target = wrap_180(angle_target) - - return angle_target + angle_target_return = wrap_180(angle_target_return) + --]] + return angle_target_return end +local pre_target_heading = 0.0 + -- main update function local function update() now = millis():tofloat() * 0.001 @@ -378,7 +422,13 @@ local function update() -- set the target frame as per user set parameter - this is fundamental to this working correctly local altitude_frame = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL local wp_loiter_rad = WP_LOITER_RAD:get() - local close_distance = airspeed_cruise * 2.0 + local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 + local fail_mode = ZPF_FAIL_MODE:get() or FLIGHT_MODE.QRTL + local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER + + LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE + OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 + TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 --[[ get the current navigation target. @@ -386,33 +436,48 @@ local function update() local target_location -- = Location() of the target local target_location_offset local target_velocity -- = Vector3f() -- velocity of lead vehicle + local target_velocity_offset --local target_velocity_offset -- = Vector3f() -- velocity of lead vehicle local target_distance -- = Vector3f() -- vector to lead vehicle --local target_offsets -- = Vector3f() -- vector to lead vehicle + offsets local target_distance_offsets -- vector to the target taking offsets into account + local xy_dist = 0.0 + local target_heading = 0.0 -- heading of the target vehicle + local heading_to_target -- heading of the follow vehicle to the target with offsets local vehicle_airspeed = ahrs:airspeed_estimate() local current_target = vehicle:get_target_location() - -- because of the methods available on AP_Follow, need to call these two methods + -- because of the methods available on AP_Follow, need to call these multiple methods get_target_dist_and_vel_ned() MUST BE FIRST -- to get target_location, target_velocity, target distance and target -- and yes target_offsets (hopefully the same value) is returned by both methods -- even worse - both internally call get_target_location_and_Velocity, but making a single method -- in AP_Follow is probably a high flash cost, so we just take the runtime hit - target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() -- this has to be first + --[[ + target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() -- THIS HAS TO BE FIRST 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() + --]] + + target_distance, target_distance_offsets, + target_velocity, target_velocity_offset, + target_location, target_location_offset, + xy_dist, heading_to_target = follow:get_target_info() + target_heading = follow:get_target_heading_deg() or -400 + --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, + --float &target_heading_ofs_deg + --) + if target_location == nil or target_velocity == nil or target_distance_offsets == nil or current_target == nil then lost_target_countdown = lost_target_countdown - 1 if lost_target_countdown <= 0 then gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") - if FOLL_FAIL_MODE:get() ~= nil then - vehicle:set_mode(FOLL_FAIL_MODE:get()) - else - vehicle:set_mode(FLIGHT_MODE.LOITER) - end + vehicle:set_mode(fail_mode) follow_enabled = false return end @@ -427,10 +492,57 @@ local function update() -- have a good target so reset the countdown lost_target_countdown = LOST_TARGET_TIMEOUT end - - local target_angle = follow_target_angle(target_heading, target_location) - local offset_angle = follow_target_angle(target_heading, target_location_offset) - local desired_heading = target_heading + + --[[ + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": tgt hdg:%.0f", target_heading)) + if pre_target_heading ~- nil then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": PRE tgt hdg:%.0f", pre_target_heading)) + if pre_target_heading ~- target_heading then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0f pre hdg: %.0f", target_heading, pre_target_heading)) + pre_target_heading = target_heading + end + end + pre_target_heading = target_heading + --]] + + -- target_angle is the difference between the current heading of the target vehicle and the follow vehicle heading to the target_location + -- local target_angle = follow_target_angle(target_heading, target_location) + -- offset_angle is the difference between the current heading of the follow vehicle and the target_location (with offsets) + --local vehicle_heading = math.deg(current_location:get_bearing(target_location)) + local vehicle_heading = math.deg(ahrs:get_yaw()) + local follow_heading = heading_to_target -- math.deg(current_location:get_bearing(target_location_offset)) + local offset_angle = wrap_180(vehicle_heading - follow_heading) + +-- local offset_angle = follow_target_angle(vehicle_heading, target_location_offset) + + -- default the desired heading to the current heading - we might change this below + local desired_heading = vehicle_heading + + -- target angle is the difference between the heading of the target and what its heading was 2 seconds ago + local target_angle = 0.0 + if target_heading ~= nil then + --if math.abs(save_target_heading1 - target_heading ) > 10.0 or math.abs( save_target_heading2- target_heading ) > 10.0 then + -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0f pre hdg1: %.0f", target_heading, save_target_heading1)) + -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0f pre hdg2: %.0f", target_heading, save_target_heading2)) + --end + -- want the greatest angle of we might have turned + local angle_diff1 = wrap_180(math.abs(save_target_heading1 - target_heading)) + local angle_diff2 = wrap_180(math.abs(save_target_heading2 - target_heading)) + if angle_diff2 > angle_diff1 then + target_angle = angle_diff2 + else + target_angle = angle_diff1 + end + if math.abs(save_target_heading1 - target_heading ) > 10.0 or math.abs( save_target_heading2- target_heading ) > 10.0 then + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0fangle: %.0f", target_heading, target_angle)) + end + -- remember the target heading from 2 seconds ago, so we can tell if it is turning or not + if (now - now_target_heading) > 1 then + save_target_heading2 = save_target_heading1 + save_target_heading1 = target_heading + now_target_heading = now + end + end local target_airspeed = target_velocity:length() local desired_airspeed = target_airspeed @@ -540,7 +652,7 @@ local function update() where = "OVERSHOT" end --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": %s distance %.0f projected %.0f ", where, xy_dist, projected_distance)) - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": %s target %.1f desired airspeed %.1f", where, target_airspeed, desired_airspeed)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": %s target %.1f desired airspeed %.1f", where, target_airspeed, desired_airspeed)) now_distance = millis():tofloat() * 0.001 end else @@ -560,7 +672,7 @@ local function update() desired_airspeed = vehicle_airspeed + ((airspeed_max - vehicle_airspeed) * distance_error) if math.floor(now_distance) ~= math.floor(now) then --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG distance %.0f projected %.0f close %.0f", xy_dist, projected_distance, close_distance)) - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG target %.1f desired airspeed %.1f", target_airspeed, desired_airspeed)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG target %.1f desired airspeed %.1f", target_airspeed, desired_airspeed)) now_distance = millis():tofloat() * 0.001 end end @@ -578,26 +690,47 @@ local function update() new_target_location:lat(target_location_offset:lat()) new_target_location:lng(target_location_offset:lng()) new_target_location:alt(target_location_offset:alt()) -- location uses cm for altitude - -- if the target is turning or we are a fair way away from it then we use location, otherwise we use heading - if (math.abs(xy_dist) <= wp_loiter_rad and math.abs(target_angle) < TURNING_ANGLE) - or math.abs(xy_dist) > close_distance then + -- if we are < close_distance we always use heading, unless the target is turning otherwise + -- if we are > close_distance and < wp_loiter_rad and the target is not turning then we use heading + -- if we are > close_distance and < wp_loiter_rad and the target is not overshot then we use heading + -- otherwise of > wp_loiter_rad if the target is turning or we are a fair way away from the target then we use location, + -- otherwise we use heading + local mechanism = 1 -- for logging 1: position/location 2:heading + local normalized_distance = math.abs(projected_distance) + if (normalized_distance > close_distance and + ((math.abs(target_angle) > TURNING_ANGLE and normalized_distance < wp_loiter_rad) or + normalized_distance > wp_loiter_rad or math.abs(offset_angle) < OVERSHOOT_ANGLE)) then +-- (math.abs(xy_dist) <= wp_loiter_rad and math.abs(target_angle) < TURNING_ANGLE) +-- or math.abs(xy_dist) > close_distance then set_vehicle_target_location({lat = target_location_offset:lat(), lng = target_location_offset:lng(), alt = target_altitude, frame = altitude_frame, turning_angle = target_angle}) + --if normalized_distance > wp_loiter_rad then + -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" normalized: %.0f projected %.0f angle target %.1f offset %.1f", normalized_distance, projected_distance, target_angle, offset_angle)) + --end + if math.abs(target_angle) > TURNING_ANGLE then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" TURNING position: %.0f save heading %.0f angle target %.1f offset %.1f", target_heading, save_target_heading1, target_angle, offset_angle)) + end + mechanism = 1 -- position/location else + if math.abs(target_angle) > TURNING_ANGLE then + desired_heading = target_heading + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" TURNING heading: %.0f save heading %.0f angle target %.1f offset %.1f", target_heading, save_target_heading1, target_angle, offset_angle)) + end set_vehicle_heading({heading = desired_heading}) set_vehicle_target_altitude({alt = target_altitude, frame = altitude_frame}) -- pass altitude in meters (location has it in cm) + mechanism = 2 -- heading end local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed) if math.floor(now_results) ~= math.floor(now) then --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format("vehicle x %.1f y %.1f length %.1f", vehicle_vector:x(), vehicle_vector:y(), vehicle_vector:length())) --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format("target x %.1f y %.1f length %.1f", target_vector:x(), target_vector:y(), target_vector:length())) - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format("xy_dist: %.0f projected %.0f angle target %.1f offset %.1f", xy_dist, projected_distance, target_angle, offset_angle)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format("normalized: %.0f projected %.0f angle target %.1f offset %.1f", normalized_distance, projected_distance, target_angle, offset_angle)) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f diff %.0f err %.3f ", xy_dist, projected_distance, distance, distance_error)) - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f asp %.1f NEW %.1f ", xy_dist, projected_distance, desired_airspeed, airspeed_new)) - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": hdg:%.0f alt: %.0f ang off %.0f trn %.0f", target_heading, target_location:alt() * 0.01, offset_angle, target_angle )) + -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f asp %.1f NEW %.1f ", xy_dist, projected_distance, desired_airspeed, airspeed_new)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": tgt hdg:%.0f veh hdg: %.0f ang off %.0f ang tar %.0f", target_heading, vehicle_heading, offset_angle, target_angle )) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target alt: %.1f", target_location:alt() * 0.01 )) if math.abs(xy_dist) < wp_loiter_rad and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" REVERSE: distance %.1f offset_angle %.1f ", xy_dist, offset_angle )) @@ -607,15 +740,19 @@ local function update() --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": distance %f speed %s heading:%f alt: %f", xy_dist, desired_airspeed, target_heading, target_location:alt() )) set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) - logger.write("ZPFL",'Dst,DstP,AspT,Asp,AspO,Hdg,Alt,AltT','ffffffff', + logger.write("ZPFL",'Dst,DstP,AspT,Asp,AspO,Hdg,AngT,AngO,Alt,AltT,Mech','ffffffffffB', xy_dist, projected_distance, target_airspeed, vehicle_airspeed, desired_airspeed, - target_heading, + target_heading, + target_angle, + offset_angle, current_altitude, - target_altitude) + target_altitude, + mechanism + ) end -- wrapper around update(). This calls update() at 20Hz, diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 21f67fcd529d1..03d01adae2dd8 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3532,12 +3532,6 @@ function follow:get_target_location_and_velocity() end ---@return Vector3f_ud|nil -- velocity function follow:get_target_location_and_velocity_ofs() end --- desc get distance vector to target (in meters) and target's velocity all in NED frame ----@return Vector3f_ud|nil ----@return Vector3f_ud|nil ----@return Vector3f_ud|nil -function follow:get_target_dist_and_vel_ned() end - -- desc ---@return uint32_t_ud function follow:get_last_update_ms() end @@ -3546,6 +3540,17 @@ function follow:get_last_update_ms() end ---@return boolean function follow:have_target() end +-- combo function returning all follow values calcuted in a cycle +---@return Vector3f_ud|nil -- distance to the target +---@return Vector3f_ud|nil -- distance to the target with offsets +---@return Vector3f_ud|nil -- proposed velocity of the target +---@return Vector3f_ud|nil -- proposed velocity of the target with offsets +---@return Location_ud|nil -- location of the target +---@return Location_ud|nil -- location of the target with offsets +---@return number|nil -- distance to the target in meters +---@return number|nil -- heading to the target with offsets in degrees +function follow:get_target_info() end + -- desc scripting = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 25c9b9d066f85..c969dfbe5d629 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -748,6 +748,8 @@ singleton AP_Follow method get_target_location_and_velocity boolean Location'Nul singleton AP_Follow method get_target_location_and_velocity_ofs boolean Location'Null Vector3f'Null singleton AP_Follow method get_target_dist_and_vel_ned boolean Vector3f'Null Vector3f'Null Vector3f'Null singleton AP_Follow method get_target_heading_deg boolean float'Null +singleton AP_Follow method get_target_info boolean Vector3f'Null Vector3f'Null Vector3f'Null Vector3f'Null Location'Null Location'Null float'Null float'Null +singleton AP_Follow method get_target_info depends APM_BUILD_TYPE(APM_BUILD_ArduPlane) include AC_PrecLand/AC_PrecLand.h singleton AC_PrecLand depends AC_PRECLAND_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI) From 16bd2e00ceb05f7c763c0c4ff6772e9f06a7cdca Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 14 Sep 2024 12:53:09 -0600 Subject: [PATCH 14/20] AP_Scripting: Scripted follow in Plane --- libraries/AP_Scripting/docs/docs.lua | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 03d01adae2dd8..ddd836efdd939 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3518,7 +3518,7 @@ singleton AP_Follow method get_target_sysid uint8_t ---@return float function follow:get_distance_to_target() end --- desc +-- get target's heading in degrees (0 = north, 90 = east) ---@return number|nil function follow:get_target_heading_deg() end @@ -3541,14 +3541,14 @@ function follow:get_last_update_ms() end function follow:have_target() end -- combo function returning all follow values calcuted in a cycle ----@return Vector3f_ud|nil -- distance to the target ----@return Vector3f_ud|nil -- distance to the target with offsets ----@return Vector3f_ud|nil -- proposed velocity of the target ----@return Vector3f_ud|nil -- proposed velocity of the target with offsets ----@return Location_ud|nil -- location of the target ----@return Location_ud|nil -- location of the target with offsets ----@return number|nil -- distance to the target in meters ----@return number|nil -- heading to the target with offsets in degrees +---@return dist_ned Vector3f_ud|nil -- distance to the target +---@return dist_with_offs Vector3f_ud|nil -- distance to the target with offsets +---@return target_vel_ned Vector3f_ud|nil -- proposed velocity of the target +---@return target_vel_ned_ofs Vector3f_ud|nil -- proposed velocity of the target with offsets +---@return target_loc Location_ud|nil -- location of the target +---@return target_loc_ofs Location_ud|nil -- location of the target with offsets +---@return target_dist_ofs number|nil -- distance to the target in meters +---@return target_heading_ofs_deg number|nil -- heading to the target with offsets in degrees function follow:get_target_info() end -- desc From b780c31578cc30b3b1dee67305ec44b84b696c2f Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 14 Sep 2024 13:05:38 -0600 Subject: [PATCH 15/20] AP_Scripting: Scripted follow in Plane --- libraries/AP_Scripting/docs/docs.lua | 4 ---- libraries/AP_Scripting/generator/description/bindings.desc | 2 -- 2 files changed, 6 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index ddd836efdd939..53d6c8a0f6334 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3514,10 +3514,6 @@ function follow:have_target() end function follow:get_target_sysid() end singleton AP_Follow method get_target_sysid uint8_t --- get horizontal distance to target (including offset) in meters (for reporting purposes) ----@return float -function follow:get_distance_to_target() end - -- get target's heading in degrees (0 = north, 90 = east) ---@return number|nil function follow:get_target_heading_deg() end diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c969dfbe5d629..916543c9263f8 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -742,11 +742,9 @@ singleton AP_Follow depends AP_FOLLOW_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduP singleton AP_Follow rename follow singleton AP_Follow method have_target boolean singleton AP_Follow method get_target_sysid uint8_t -singleton AP_Follow method get_distance_to_target float singleton AP_Follow method get_last_update_ms uint32_t singleton AP_Follow method get_target_location_and_velocity boolean Location'Null Vector3f'Null singleton AP_Follow method get_target_location_and_velocity_ofs boolean Location'Null Vector3f'Null -singleton AP_Follow method get_target_dist_and_vel_ned boolean Vector3f'Null Vector3f'Null Vector3f'Null singleton AP_Follow method get_target_heading_deg boolean float'Null singleton AP_Follow method get_target_info boolean Vector3f'Null Vector3f'Null Vector3f'Null Vector3f'Null Location'Null Location'Null float'Null float'Null singleton AP_Follow method get_target_info depends APM_BUILD_TYPE(APM_BUILD_ArduPlane) From c327fa65acadfff474ef7bebc9244371e895bf9d Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 14 Sep 2024 15:59:19 -0600 Subject: [PATCH 16/20] AP_Follow: Scripted follow in Plane --- libraries/AP_Follow/AP_Follow.cpp | 5 ++--- libraries/AP_Follow/AP_Follow.h | 7 ++++++- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 44b95cf19ef42..9ca9d513d55fb 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -254,9 +254,6 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w return true; } - - - // get target's heading in degrees (0 = north, 90 = east) bool AP_Follow::get_target_heading_deg(float &heading) const { @@ -568,6 +565,7 @@ bool AP_Follow::have_target(void) const 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() -- THIS HAS TO BE FIRST @@ -596,6 +594,7 @@ bool AP_Follow::get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs, target_heading_ofs_deg = _bearing_to_target; return true; } +#endif namespace AP { diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index cabcd735256f5..6ef7eb95d51f2 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -26,6 +26,8 @@ #include #include +#include + class AP_Follow { @@ -112,8 +114,9 @@ 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(s) + // 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, @@ -122,6 +125,8 @@ class AP_Follow float &target_dist_ofs, float &target_heading_ofs_deg ); +#endif + // parameter list static const struct AP_Param::GroupInfo var_info[]; From 378bec66a886da8ad3ce792517816d2b3ecf633b Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Sat, 14 Sep 2024 16:01:22 -0600 Subject: [PATCH 17/20] AP_Scripting: Scripted follow in Plane --- libraries/AP_Scripting/generator/description/bindings.desc | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 916543c9263f8..af2db52de231b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -22,7 +22,6 @@ userdata Location method get_distance_NE Vector2f Location userdata Location method get_alt_frame uint8_t userdata Location method change_alt_frame boolean Location::AltFrame'enum Location::AltFrame::ABSOLUTE Location::AltFrame::ABOVE_TERRAIN userdata Location method copy Location -userdata Location method line_path_proportion float Location Location include AP_AHRS/AP_AHRS.h @@ -49,7 +48,6 @@ singleton AP_AHRS method get_relative_position_D_home void float'Ref singleton AP_AHRS method home_is_set boolean singleton AP_AHRS method healthy boolean singleton AP_AHRS method airspeed_estimate boolean float'Null -singleton AP_AHRS method airspeed_vector_true boolean Vector3f'Null singleton AP_AHRS method get_vibration Vector3f singleton AP_AHRS method earth_to_body Vector3f Vector3f singleton AP_AHRS method body_to_earth Vector3f Vector3f @@ -161,7 +159,6 @@ userdata Vector3f method copy Vector3f userdata Vector3f method xy Vector2f userdata Vector3f method rotate_xy void float'skip_check userdata Vector3f method angle float Vector3f -userdata Vector3f method project void Vector3f userdata Vector2f field x float'skip_check read write userdata Vector2f field y float'skip_check read write From 45309d7927005d2a8a20d680f8428e78838109cd Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 26 Sep 2024 12:52:34 -0600 Subject: [PATCH 18/20] ArduPlane: Scripted follow in Plane --- ArduPlane/mode_guided.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index da78766622b88..435d04a79a051 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -2,7 +2,7 @@ #include "Plane.h" // In the previous version this defaults to 3000 which limits Guided from doing timely updates -#define LIMIT_ON_GUIDED_UPDATE_FREQUENCY 50 +#define LIMIT_ON_GUIDED_UPDATE_FREQUENCY 25 bool ModeGuided::_enter() { From 4f2d9b78ba241afecb75bae2b44da05db4637940 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 26 Sep 2024 12:54:02 -0600 Subject: [PATCH 19/20] AP_Scripting: Scripted follow in Plane --- libraries/AP_Follow/AP_Follow.cpp | 22 +- .../AP_Scripting/applets/plane_follow.lua | 442 +++++++++++------- 2 files changed, 284 insertions(+), 180 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 9ca9d513d55fb..85bf9390b98cd 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -35,12 +35,14 @@ extern const AP_HAL::HAL& hal; #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 +#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 +130,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 @@ -345,12 +347,10 @@ 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 (_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 + if (_alt_type == AP_FOLLOW_ALT_TYPE_DEFAULT) { _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE); + } else { + _target_location.set_alt_cm(packet.relative_alt / 10, static_cast((int)_alt_type)); } _target_velocity_ned.x = packet.vx * 0.01f; // velocity north @@ -393,8 +393,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((int)_alt_type))) { return false; } _target_location = new_loc; diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 53552a7ec628d..f676b3cc4cd76 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -25,7 +25,7 @@ ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.6.0-022" +SCRIPT_VERSION = "4.6.0-029" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" @@ -44,12 +44,17 @@ FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRT DISTANCE_LOOKAHEAD_SECONDS = 5 local current_location = ahrs:get_location() +local ahrs_eas2tas = ahrs:get_EAS2TAS() +local windspeed_vector = ahrs:wind_estimate() + local now = millis():tofloat() * 0.001 local now_too_close = now local now_overshot = now +local now_target_distance = now local now_distance = now local now_results = now local now_airspeed = now +local now_target_airspeed = now local now_lost_target = now local now_target_heading = now local follow_enabled = false @@ -58,11 +63,7 @@ local lost_target_countdown = LOST_TARGET_TIMEOUT local save_target_heading = {0.0, 0.0, 0.0} local save_target_heading1 = -400.0 local save_target_heading2 = -400.0 - --- bind a parameter to a variable -local function bind_param(name) - return Parameter(name) -end +local tight_turn = false local PARAM_TABLE_KEY = 100 local PARAM_TABLE_PREFIX = "ZPF_" @@ -70,7 +71,7 @@ local PARAM_TABLE_PREFIX = "ZPF_" -- add a parameter and bind it to a variable local function bind_add_param(name, idx, default_value) assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) - return bind_param(PARAM_TABLE_PREFIX .. name) + return Parameter(PARAM_TABLE_PREFIX .. name) end -- setup follow mode specific parameters @@ -81,10 +82,13 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add -- is no need to access them in the scriot -- we need these existing FOLL_ parametrs -FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE') -FOLL_SYSID = bind_param('FOLL_SYSID') -FOLL_OFS_Y = bind_param('FOLL_OFS_Y') -FOLL_OFS_X = bind_param('FOLL_OFS_X') +FOLL_ALT_TYPE = Parameter('FOLL_ALT_TYPE') +FOLL_SYSID = Parameter('FOLL_SYSID') +FOLL_OFS_Y = Parameter('FOLL_OFS_Y') +local foll_sysid = FOLL_SYSID:get() or -1 +local foll_ofs_y = FOLL_OFS_Y:get() or 0 +local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL + -- Add these ZPF_ parameters specifically for this script --[[ @@ -136,7 +140,7 @@ ZPF_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75) // @Range: 0 180 // @Units: degrees --]] -ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 60) +ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 15) --[[ // @Param: ZPF_DIST_CLOSE @@ -147,19 +151,39 @@ ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 60) --]] ZPF_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 40) +--[[ + // @Param: ZPF_WIDE_TURNS + // @DisplayName: Plane Follow Scripting Wide Turns + // @Description: Use wide turns when following a turning target. Alternative is "cutting the corner" + // @Range: 0 1 + // @Units: boolean +--]] +ZPF_WIDE_TURNS = bind_add_param("WIDE_TURNS", 8, 1) + +--[[ + // @Param: ZPF_ALT + // @DisplayName: Plane Follow Scripting Altitude Override + // @Description: When non zero, this altitude value (in FOLL_ALT_TYPE frame) overrides the value sent by the target vehicle + // @Range: 0 1000 + // @Units: meters +--]] +ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) + REFRESH_RATE = 0.05 -- in seconds, so 20Hz LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 -AIRSPEED_MIN = bind_param('AIRSPEED_MIN') -AIRSPEED_MAX = bind_param('AIRSPEED_MAX') -AIRSPEED_CRUISE = bind_param('AIRSPEED_CRUISE') -WP_LOITER_RAD = bind_param('WP_LOITER_RAD') +AIRSPEED_MIN = Parameter('AIRSPEED_MIN') +AIRSPEED_MAX = Parameter('AIRSPEED_MAX') +AIRSPEED_CRUISE = Parameter('AIRSPEED_CRUISE') +WP_LOITER_RAD = Parameter('WP_LOITER_RAD') +WINDSPEED_MAX = Parameter('AHRS_WIND_MAX') local airspeed_max = AIRSPEED_MAX:get() or 25.0 local airspeed_min = AIRSPEED_MIN:get() or 12.0 local airspeed_cruise = AIRSPEED_CRUISE:get() or 18.0 +local windspeed_max = WINDSPEED_MAX:get() or 100.0 --[[ create a NaN value @@ -178,7 +202,10 @@ local function constrain(v, vmin, vmax) end local speedpi = require("speedpi") -local speed_controller = speedpi.speed_controller(0.1, 0.1, 5.0, airspeed_min, airspeed_max) +local speed_controller = speedpi.speed_controller(0.2, 0.2, 5.0, airspeed_min, airspeed_max) + +local mavlink_attitude = require("mavlink_attitude") +local mavlink_attitude_receiver = mavlink_attitude.mavlink_attitude_receiver() local function follow_frame_to_mavlink(follow_frame) local mavlink_frame = MAV_FRAME.GLOBAL; @@ -191,24 +218,14 @@ local function follow_frame_to_mavlink(follow_frame) return mavlink_frame end -local now_altitude = millis():tofloat() * 0.001 --- target.alt = new target altitude in meters -- set_vehicle_target_altitude() Parameters +-- target.alt = new target altitude in meters -- target.frame = Altitude frame MAV_FRAME, it's very important to get this right! -- target.alt = altitude in meters to acheive --- target.accel = z acceleration to altitude (1000.0 = max) +-- target.speed = z speed of change to altitude (1000.0 = max) local function set_vehicle_target_altitude(target) local home_location = ahrs:get_home() - local acceleration = target.accel or 1000.0 -- default to maximum z acceleration - if math.floor(now) ~= math.floor(now_altitude) then - now_altitude = millis():tofloat() * 0.001 - --[[ - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": set heading alt: %.1f", target.alt) ) - if home_location ~= nil then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(" set location : alt home relative: %.1f", (target.alt - (home_location:alt() * 0.01)))) - end - --]] - end + local speed = target.speed or 1000.0 -- default to maximum z acceleration if target.alt == nil then gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude") return @@ -216,13 +233,12 @@ local function set_vehicle_target_altitude(target) -- GUIDED_CHANGE_ALTITUDE takes altitude in meters if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { frame = follow_frame_to_mavlink(target.frame), - p3 = acceleration, + p3 = speed, z = target.alt }) then gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") end end -local now_heading = millis():tofloat() * 0.001 -- set_vehicle_heading() Parameters -- heading.heading_type = MAV_HEADING_TYPE (defaults to HEADING) -- heading.heading = the target heading in degrees @@ -232,10 +248,6 @@ local function set_vehicle_heading(heading) local heading_heading = heading.heading or 0 local heading_accel = heading.accel or 0.0 - if heading_type ~= MAV_HEADING_TYPE.DEFAULT and math.floor(now) ~= math.floor(now_heading) then - now_heading = millis():tofloat() * 0.001 - end - if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL, p1 = heading_type, p2 = heading_heading, @@ -286,18 +298,10 @@ local function set_vehicle_target_location(target) local angle = target.turning_angle or 0 local yaw = target.yaw or 1 -- If we are on the right side of the vehicle make sure any loitering is CCW (moves away from the other plane) - if FOLL_OFS_Y:get() > 0 or angle < 0 then + if foll_ofs_y > 0 or angle < 0 then yaw = -yaw end - if math.floor(now) ~= math.floor(now_altitude) then - now_altitude = millis():tofloat() * 0.001 - if home_location ~= nil then - --gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(" set location : alt home relative: %.1f", (target.alt - (home_location:alt() * 0.01)))) - end - --gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. string.format(" set location : alt: %.1f", target.alt)) - end - -- if we were in HEADING mode, need to switch out of it so that REPOSITION will work -- Note that MAVLink DO_REPOSITION requires altitude in meters set_vehicle_heading({type = MAV_HEADING_TYPE.DEFAULT}) @@ -391,6 +395,7 @@ local function wrap_180(angle) return res end +--[[ probably not required -- calculate difference between the target heading and the following vehicle heading local function follow_target_angle(target_heading_follow, target_location_follow) @@ -398,21 +403,32 @@ local function follow_target_angle(target_heading_follow, target_location_follow local follow_heading = math.deg(current_location:get_bearing(target_location_follow)) local angle_target_return = wrap_180(target_heading_follow - follow_heading) - --[[ - if follow_heading > vehicle_heading then - angle_target_return = target_heading - vehicle_heading - end - angle_target_return = wrap_180(angle_target_return) - --]] + + --if follow_heading > vehicle_heading then + -- angle_target_return = target_heading - vehicle_heading + --end + --angle_target_return = wrap_180(angle_target_return) + return angle_target_return end +--]] + +local function calculate_airspeed_from_groundspeed(velocity_vector) + local airspeed_vector = velocity_vector - windspeed_vector + local airspeed = airspeed_vector:length() + airspeed = airspeed * ahrs_eas2tas + airspeed = constrain(airspeed, airspeed - windspeed_max, airspeed + windspeed_max) + airspeed = airspeed / ahrs_eas2tas -local pre_target_heading = 0.0 + return airspeed +end -- main update function local function update() now = millis():tofloat() * 0.001 current_location = ahrs:get_location() + ahrs_eas2tas = ahrs:get_EAS2TAS() + windspeed_vector = ahrs:wind_estimate() follow_check() if not follow_active() then @@ -420,30 +436,32 @@ local function update() end -- set the target frame as per user set parameter - this is fundamental to this working correctly - local altitude_frame = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL local wp_loiter_rad = WP_LOITER_RAD:get() local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 + local long_distance = close_distance * 4.0 local fail_mode = ZPF_FAIL_MODE:get() or FLIGHT_MODE.QRTL local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER + local use_wide_turns = ZPF_WIDE_TURNS:get() or 1 + local altitude_override = ZPF_ALT_OVR:get() or 0 LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 + foll_ofs_y = FOLL_OFS_Y:get() or 0 + foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL --[[ get the current navigation target. --]] local target_location -- = Location() of the target - local target_location_offset - local target_velocity -- = Vector3f() -- velocity of lead vehicle - local target_velocity_offset - --local target_velocity_offset -- = Vector3f() -- velocity of lead vehicle + local target_location_offset -- Location of the target with FOLL_OFS_* offsets applied + local target_velocity -- = Vector3f() -- current velocity of lead vehicle + local target_velocity_offset -- Vector3f -- velocity to the offset target_location_offset local target_distance -- = Vector3f() -- vector to lead vehicle - --local target_offsets -- = Vector3f() -- vector to lead vehicle + offsets - local target_distance_offsets -- vector to the target taking offsets into account - local xy_dist = 0.0 - local target_heading = 0.0 -- heading of the target vehicle - local heading_to_target -- heading of the follow vehicle to the target with offsets + local target_distance_offsets -- vector to the target taking offsets into account + local xy_dist = 0.0 -- distance to target with offsets in meters + local target_heading = 0.0 -- heading of the target vehicle + local heading_to_target -- heading of the follow vehicle to the target with offsets local vehicle_airspeed = ahrs:airspeed_estimate() local current_target = vehicle:get_target_location() @@ -466,65 +484,71 @@ local function update() target_location, target_location_offset, xy_dist, heading_to_target = follow:get_target_info() target_heading = follow:get_target_heading_deg() or -400 - --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, - --float &target_heading_ofs_deg - --) + -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to reaquire it if target_location == nil or target_velocity == nil or target_distance_offsets == nil or current_target == nil then lost_target_countdown = lost_target_countdown - 1 if lost_target_countdown <= 0 then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") - vehicle:set_mode(fail_mode) follow_enabled = false + vehicle:set_mode(fail_mode) + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") return end if math.floor(now) ~= math.floor(now_lost_target) then now_lost_target = millis():tofloat() * 0.001 gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": lost target: " .. FOLL_SYSID:get() .. " count: " .. lost_target_countdown) end - -- if we have temporarily lost the target, maintain the heading of the target while we wait for LOST_TARGET_TIMEOUT seconds to re-aquire it - -- set_vehicle_heading({heading = target_heading}) return else -- have a good target so reset the countdown lost_target_countdown = LOST_TARGET_TIMEOUT end + -- target_velocity from MAVLink (via AP_Follow) is groundspeed, need to convert to airspeed, + -- we can only assume the windspeed for the target is the same as the chase plane + --[[ + This is the code from AHRS.cpp + Vector3f true_airspeed_vec = nav_vel - wind_vel; + + This is the c++ code from AP_AHRS_DCM.cpp and also from AP_AHRS.cpp + float true_airspeed = airspeed_ret * get_EAS2TAS(); + true_airspeed = constrain_float(true_airspeed, + gnd_speed - _wind_max, + gnd_speed + _wind_max); + airspeed_ret = true_airspeed / get_EAS2TAS( + --]] + local target_airspeed = calculate_airspeed_from_groundspeed(target_velocity) + --[[ - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": tgt hdg:%.0f", target_heading)) - if pre_target_heading ~- nil then - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": PRE tgt hdg:%.0f", pre_target_heading)) - if pre_target_heading ~- target_heading then - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0f pre hdg: %.0f", target_heading, pre_target_heading)) - pre_target_heading = target_heading - end + if math.floor(now) ~= math.floor(now_target_airspeed) then + now_target_airspeed = millis():tofloat() * 0.001 + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": wind %.1f target vel: %.1f asp: %.1f", windspeed_vector:length(), target_velocity:length(), target_airspeed) ) end - pre_target_heading = target_heading --]] -- target_angle is the difference between the current heading of the target vehicle and the follow vehicle heading to the target_location -- local target_angle = follow_target_angle(target_heading, target_location) -- offset_angle is the difference between the current heading of the follow vehicle and the target_location (with offsets) --local vehicle_heading = math.deg(current_location:get_bearing(target_location)) - local vehicle_heading = math.deg(ahrs:get_yaw()) + local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw()))) local follow_heading = heading_to_target -- math.deg(current_location:get_bearing(target_location_offset)) local offset_angle = wrap_180(vehicle_heading - follow_heading) --- local offset_angle = follow_target_angle(vehicle_heading, target_location_offset) - + -- rotate the target_distance_offsets in NED to the same direction has the follow vehicle, we use this below + target_distance_offsets:rotate_xy(math.rad(vehicle_heading)) + --[[ + if math.floor(now_target_distance) ~= math.floor(now) then + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": distance %.0f y %.0f ", target_distance_offsets:length(), target_distance_offsets:y())) + now_target_distance = millis():tofloat() * 0.001 + end + --]] + -- default the desired heading to the current heading - we might change this below - local desired_heading = vehicle_heading + local desired_heading = target_heading -- target angle is the difference between the heading of the target and what its heading was 2 seconds ago local target_angle = 0.0 - if target_heading ~= nil then - --if math.abs(save_target_heading1 - target_heading ) > 10.0 or math.abs( save_target_heading2- target_heading ) > 10.0 then - -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0f pre hdg1: %.0f", target_heading, save_target_heading1)) - -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": DIFFERENT tgt hdg:%.0f pre hdg2: %.0f", target_heading, save_target_heading2)) - --end + if target_heading ~= nil and target_heading > -400 then -- want the greatest angle of we might have turned local angle_diff1 = wrap_180(math.abs(save_target_heading1 - target_heading)) local angle_diff2 = wrap_180(math.abs(save_target_heading2 - target_heading)) @@ -544,30 +568,63 @@ local function update() end end - local target_airspeed = target_velocity:length() + -- if the target vehicle is starting to roll we need to pre-empt a turn is coming + -- this is fairly simplistic and could probably be improved + -- got the code from Mission Planner - this is how it calculates the turn radius + --[[ + public float radius + { + get + { + if (_groundspeed <= 1) return 0; + return (float)toDistDisplayUnit(_groundspeed * _groundspeed / (9.80665 * Math.Tan(roll * MathHelper.deg2rad))); + } + } + --]] + local target_attitude = mavlink_attitude_receiver.get_attitude(foll_sysid) + if target_attitude ~= nil then + if math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then + local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll)) + local angle_adjustment = 60 * target_attitude.roll + -- predict the roll in 1s from now and use that. + -- this is wrong - this is the roll angle not the turn angle + -- need some more maths to convert a roll angle into a turn angle + turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed)) + local tangent_angle = wrap_360(math.deg(math.pi/2.0 - vehicle_airspeed / turn_radius)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": Att roll:%.2f rollspd: %.2f rad %.2f tan %.2f", target_attitude.roll, target_attitude.rollspeed, turn_radius, tangent_angle)) + + angle_adjustment = tangent_angle + target_angle = wrap_360(target_angle + angle_adjustment) + target_heading = wrap_360(target_heading + angle_adjustment) + -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": adjust:%.2f angle: %.2f heading %.2f", angle_adjustment, target_angle, target_heading)) + -- push the target heading back because it hasn't figured out we are turning yet + save_target_heading1 = save_target_heading2 + end + -- if the roll direction is the same as the y offset, then we are turning on the "inside" (a tight turn) but + -- if rollspeed direction is not the same as roll direction it means we are comming out of the turn + if (target_attitude.roll < 0 and target_attitude.rollspeed <=0 and foll_ofs_y < 0) or + (target_attitude.roll > 0 and target_attitude.rollspeed >=0 and foll_ofs_y > 0) then + tight_turn = true + else + tight_turn = false + end + end + local desired_airspeed = target_airspeed local airspeed_difference = vehicle_airspeed - target_airspeed -- distance seem to be out by about 1s at approximately current airspeed just eyeballing it. xy_dist = math.abs(xy_dist) - vehicle_airspeed * 0.92 -- xy_dist will always be a positive value. To get -v to represent overshoot, use the offset_angle - -- to decide if the target is behind. - if (math.abs(xy_dist) < wp_loiter_rad) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then + -- to decide if the target is behind + if (math.abs(xy_dist) < long_distance) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then xy_dist = -xy_dist end - - local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS - -- next we try to match the airspeed of the target vehicle, calculating if we - -- need to speed up if too far behind, or slow down if too close + -- projected_distance is how far off the target we expect to be if we maintain the current airspeed for DISTANCE_LOOKAHEAD_SECONDS + local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS - -- if the current velocity will not catch up to the target then we need to speed up - -- use DISTANCE_LOOKAHEAD_SECONDS seconds as a reasonable time to try to catch up - -- first figure out how far away we will be from the required location in DISTANCE_LOOKAHEAD_SECONDS seconds if we maintain the current vehicle and target airspeeds - -- There is nothing magic about 12, it is just "what works" - - -- don't count it as close if the heading is diverging (i.e. the target plane is turning) - --local too_close = (xy_dist > 0) and (math.abs(xy_dist) < close_distance) and (offset_angle < OVERSHOOT_ANGLE) + -- don't count it as close if the heading is diverging (i.e. the target plane has overshot the target so extremely that it's pointing in the wrong direction) local too_close = (projected_distance > 0) and (math.abs(projected_distance) < close_distance) and (offset_angle < OVERSHOOT_ANGLE) if too_close then if math.floor(now_too_close) ~= math.floor(now) then @@ -579,9 +636,13 @@ local function update() -- we've overshot if -- the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS (projected_distance) - -- or the distance to the target location is already negative AND - -- the target is very close OR the angle to the target plane is effectively backwards - local overshot = not too_close and (projected_distance < 0 or xy_dist < 0) and (math.abs(xy_dist) < airspeed_max * 2.0) + -- or the distance to the target location is already negative AND the target is very close OR + -- the angle to the target plane is effectively backwards + local overshot = not too_close and ((projected_distance < 0 or xy_dist < 0) and (math.abs(xy_dist) < close_distance) + or offset_angle > OVERSHOOT_ANGLE + ) + + --[[ if overshot then if math.floor(now_overshot) ~= math.floor(now) then now_overshot = millis():tofloat() * 0.001 @@ -594,52 +655,45 @@ local function update() --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT reason distance: %.0f project %0.f close: %.0f", xy_dist, projected_distance, close_distance * 2)) end end - -- xy 25 proj 20 = 5 - -- xy -25 proj -20 = 5 - -- xy -5 proj 5 = 10 - -- xy 5 proj -5 = 10 - -- xy 23 proj 34 = 10 - local distance = math.abs(xy_dist - projected_distance) + --]] + local distance_error = constrain(math.abs(projected_distance) / (close_distance * DISTANCE_LOOKAHEAD_SECONDS), 0.0, 1.0) + local speed_factor = 8.0 -- how agressively to scale the speed 1.0 - 8.0 least to most -ve means go slower if overshot or too_close or too_close_follow_up > 0 then - -- special cases if we have overshot or are just about to hit the target, - -- if we are too close the target_angle will likely be wrong desired_heading = target_heading - if too_close_follow_up > 0 then - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOO CLOSE follow up: %d", too_close_follow_up) ) - end -- now calculate what airspeed we will need to fly for a few seconds to catch up the projected distance -- need to slow down dramatically. First we try to match the TARGET airspeed unless special cases kick in if overshot then - -- if we aren't going fast enough speed up, otherwise stay the course - - --if math.abs(projected_distance) < X_FUDGE then -- ignore it, close enough - -- desired_airspeed = target_airspeed - --else - if projected_distance < 0 then -- going too fast - slow down - desired_airspeed = vehicle_airspeed - (((vehicle_airspeed - airspeed_min) * distance_error) / 2.0) - else -- project that we are going too slow - speed up a bit but not too much - desired_airspeed = vehicle_airspeed - (((airspeed_max - vehicle_airspeed) * distance_error) / 8.0) + -- either we have actually overshot or we are likely to overshot (projected) very soon + -- but if we have overshot actual xy_dist, but projected is not overshot then we are already recovering + if xy_dist < 0 and projected_distance < 0 then -- actual and projected overshoot + speed_factor = -3.0 + elseif xy_dist > 0 and projected_distance < 0 then -- actual ok, but projected overshoot + speed_factor = -1.5 + elseif xy_dist < 0 and projected_distance > 0 then -- we are probably recovering + speed_factor = -1.0 + else -- we get here if we used the OVERSHOOT_ANGLE to decide we overshot. The distances are fine, so don't change speed + speed_factor = 0.0 end if math.floor(now_airspeed) ~= math.floor(now) then now_airspeed = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT airspeed projected %.0f desired speed: %.1f error: %.3f", projected_distance, desired_airspeed, distance_error) ) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": OVERSHOT airspeed projected %.0f desired speed: %.1f error: %.3f", projected_distance, desired_airspeed, distance_error) ) end - else too_close = true -- slow down so we don't overshoot - if -xy_dist < -projected_distance or xy_dist < 0 or projected_distance < 0 then -- slow down a bit more than, we are going too fast - desired_airspeed = vehicle_airspeed - (((vehicle_airspeed - airspeed_min) * distance_error) / 4.0) - else - desired_airspeed = vehicle_airspeed + (((vehicle_airspeed - airspeed_min) * distance_error) / 8.0) + if xy_dist < 0 or projected_distance < 0 then -- slow down a bit more than, we are going too fast + speed_factor = -2.0 + elseif xy_dist < projected_distance then -- getting further away need to speed up a bit + speed_factor = 3.0 + else -- this is good - we are catching up - "just right" + speed_factor = 1.0 end if math.floor(now_airspeed) ~= math.floor(now) then now_airspeed = millis():tofloat() * 0.001 - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOOCLOSE dst %.0f prj %0.f asp new: %.1f err: %.3f", xy_dist, projected_distance, desired_airspeed, distance_error) ) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOOCLOSE dst %.0f prj %0.f asp new: %.1f err: %.3f", xy_dist, projected_distance, desired_airspeed, distance_error) ) end - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": TOO CLOSE desired speed: %.1f", desired_airspeed) ) end if too_close_follow_up > 0 then too_close_follow_up = too_close_follow_up - 1 @@ -655,6 +709,13 @@ local function update() --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": %s target %.1f desired airspeed %.1f", where, target_airspeed, desired_airspeed)) now_distance = millis():tofloat() * 0.001 end + if speed_factor == 0 then + desired_airspeed = vehicle_airspeed + elseif speed_factor < 0 then -- go slower - down to airspeed_min + desired_airspeed = vehicle_airspeed + (((vehicle_airspeed - airspeed_min) * distance_error) / speed_factor) * 8.0 + else -- go faster - up to airspeed_max + desired_airspeed = vehicle_airspeed + (((airspeed_max - vehicle_airspeed) * distance_error) / speed_factor) * 8.0 + end else too_close_follow_up = 0 -- AP_Follow doesn't speed up enough if wa are a long way from the target @@ -663,18 +724,17 @@ local function update() if projected_distance > 0 then local incremental_speed = projected_distance / DISTANCE_LOOKAHEAD_SECONDS desired_airspeed = constrain(target_airspeed + incremental_speed, airspeed_min, airspeed_max) - -- desired_airspeed = target_airspeed + (airspeed_max - target_airspeed) * (projected_distance - xy_dist) /xy_dist * AIRSPEED_GAIN - -- gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": LONG LONG projected %.0f target %.1f desired %.1f", projected_distance, target_airspeed, desired_airspeed)) else desired_airspeed = target_airspeed + 2 end - --distance_error = constrain(math.abs((-xy_dist + projected_distance)) / airspeed_cruise * DISTANCE_LOOKAHEAD_SECONDS / 2.0, 0.0, 1.0) - desired_airspeed = vehicle_airspeed + ((airspeed_max - vehicle_airspeed) * distance_error) + --desired_airspeed = vehicle_airspeed + ((airspeed_max - vehicle_airspeed) * distance_error) + --[[ if math.floor(now_distance) ~= math.floor(now) then --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG distance %.0f projected %.0f close %.0f", xy_dist, projected_distance, close_distance)) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": LONG target %.1f desired airspeed %.1f", target_airspeed, desired_airspeed)) now_distance = millis():tofloat() * 0.001 end + --]] end local old_location = ahrs:get_location() @@ -686,20 +746,46 @@ local function update() local new_target_location = old_location:copy() local target_altitude = 0.0 - target_altitude = target_location_offset:alt() * 0.01 + if altitude_override ~= 0 then + target_altitude = altitude_override + else + -- change the incoming altitude frame from the target_vehicle to the frame this vehicle wants + target_location_offset:change_alt_frame(foll_alt_type) + target_altitude = target_location_offset:alt() * 0.01 + end new_target_location:lat(target_location_offset:lat()) new_target_location:lng(target_location_offset:lng()) new_target_location:alt(target_location_offset:alt()) -- location uses cm for altitude - -- if we are < close_distance we always use heading, unless the target is turning otherwise - -- if we are > close_distance and < wp_loiter_rad and the target is not turning then we use heading - -- if we are > close_distance and < wp_loiter_rad and the target is not overshot then we use heading - -- otherwise of > wp_loiter_rad if the target is turning or we are a fair way away from the target then we use location, - -- otherwise we use heading + local mechanism = 1 -- for logging 1: position/location 2:heading local normalized_distance = math.abs(projected_distance) - if (normalized_distance > close_distance and - ((math.abs(target_angle) > TURNING_ANGLE and normalized_distance < wp_loiter_rad) or - normalized_distance > wp_loiter_rad or math.abs(offset_angle) < OVERSHOOT_ANGLE)) then + local turning = normalized_distance < long_distance and math.abs(target_angle) > TURNING_ANGLE + local too_wide = math.abs(target_distance_offsets:y()) > (close_distance/3) and not turning + local wide_turn = not tight_turn + + -- xy_dist < 3.0 is a special case because mode_guided will try to loiter around the target location if within 2m + if (turning and (tight_turn or (wide_turn and not use_wide_turns) or foll_ofs_y == 0)) or + (too_close and not too_wide) or overshot or + math.abs(xy_dist) < 3.0 then + set_vehicle_heading({heading = desired_heading}) + set_vehicle_target_altitude({alt = target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + mechanism = 2 -- heading - for logging + else + set_vehicle_target_location({lat = target_location_offset:lat(), + lng = target_location_offset:lng(), + alt = target_altitude, + frame = foll_alt_type, + turning_angle = target_angle}) + mechanism = 1 -- position/location - for logging + end + + --[[ + if too_wide or + (not turning and + (normalized_distance > close_distance and + (turning or + normalized_distance > wp_loiter_rad or + math.abs(offset_angle) < OVERSHOOT_ANGLE))) then -- (math.abs(xy_dist) <= wp_loiter_rad and math.abs(target_angle) < TURNING_ANGLE) -- or math.abs(xy_dist) > close_distance then set_vehicle_target_location({lat = target_location_offset:lat(), @@ -710,49 +796,68 @@ local function update() --if normalized_distance > wp_loiter_rad then -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" normalized: %.0f projected %.0f angle target %.1f offset %.1f", normalized_distance, projected_distance, target_angle, offset_angle)) --end - if math.abs(target_angle) > TURNING_ANGLE then - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" TURNING position: %.0f save heading %.0f angle target %.1f offset %.1f", target_heading, save_target_heading1, target_angle, offset_angle)) - end - mechanism = 1 -- position/location + --if math.abs(target_angle) > TURNING_ANGLE then + -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" TURNING position: %.0f save heading %.0f angle target %.1f offset %.1f", target_heading, save_target_heading1, target_angle, offset_angle)) + --end + mechanism = 1 -- position/location - for logging else - if math.abs(target_angle) > TURNING_ANGLE then - desired_heading = target_heading - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" TURNING heading: %.0f save heading %.0f angle target %.1f offset %.1f", target_heading, save_target_heading1, target_angle, offset_angle)) - end + --if math.abs(target_angle) > TURNING_ANGLE then + -- desired_heading = target_heading + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" TURNING heading: %.0f save heading %.0f angle target %.1f offset %.1f", target_heading, save_target_heading1, target_angle, offset_angle)) + --end set_vehicle_heading({heading = desired_heading}) set_vehicle_target_altitude({alt = target_altitude, frame = altitude_frame}) -- pass altitude in meters (location has it in cm) - mechanism = 2 -- heading + mechanism = 2 -- heading - for logging end + ]]-- local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed) if math.floor(now_results) ~= math.floor(now) then --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format("vehicle x %.1f y %.1f length %.1f", vehicle_vector:x(), vehicle_vector:y(), vehicle_vector:length())) --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format("target x %.1f y %.1f length %.1f", target_vector:x(), target_vector:y(), target_vector:length())) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format("normalized: %.0f projected %.0f angle target %.1f offset %.1f", normalized_distance, projected_distance, target_angle, offset_angle)) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f diff %.0f err %.3f ", xy_dist, projected_distance, distance, distance_error)) - -- gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f asp %.1f NEW %.1f ", xy_dist, projected_distance, desired_airspeed, airspeed_new)) - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": tgt hdg:%.0f veh hdg: %.0f ang off %.0f ang tar %.0f", target_heading, vehicle_heading, offset_angle, target_angle )) - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target alt: %.1f", target_location:alt() * 0.01 )) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f asp %.1f NEW %.1f fac %.1f ", xy_dist, projected_distance, desired_airspeed, airspeed_new, speed_factor)) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": tgt hdg:%.0f veh hdg: %.0f des hdg %.0f ang tar %.0f", target_heading, vehicle_heading, desired_heading, target_angle )) + gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target alt: %.1f desired alt %.1f frame %d", target_location:alt() * 0.01, target_altitude, foll_alt_type )) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": distance %.0f y %.0f ", target_distance_offsets:length(), target_distance_offsets:y())) if math.abs(xy_dist) < wp_loiter_rad and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" REVERSE: distance %.1f offset_angle %.1f ", xy_dist, offset_angle )) end now_results = millis():tofloat() * 0.001 end - --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": distance %f speed %s heading:%f alt: %f", xy_dist, desired_airspeed, target_heading, target_location:alt() )) + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) - logger.write("ZPFL",'Dst,DstP,AspT,Asp,AspO,Hdg,AngT,AngO,Alt,AltT,Mech','ffffffffffB', + local log_too_close = 0 + local log_too_close_follow_up = 0 + local log_overshot = 0 + if too_close then + log_too_close = 1 + end + if too_close_follow_up then + log_too_close_follow_up = 1 + end + if overshot then + log_overshot = 1 + end + logger.write("ZPF1",'Dst,DstP,DstE,AspT,Asp,AspO,Mech,Cls,ClsF,OSht','ffffffBBBB', xy_dist, projected_distance, + distance_error, target_airspeed, vehicle_airspeed, - desired_airspeed, - target_heading, - target_angle, - offset_angle, - current_altitude, - target_altitude, - mechanism + airspeed_new, + mechanism, log_too_close, log_too_close_follow_up, log_overshot ) + logger.write("ZPF2",'AngT,AngO,Alt,AltT,HdgT,Hdg,HdgO','fffffff', + target_angle, + offset_angle, + current_altitude, + target_altitude, + target_heading, + vehicle_heading, + desired_heading + ) end -- wrapper around update(). This calls update() at 20Hz, @@ -773,4 +878,3 @@ gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_N -- start running update loop return protected_wrapper() - From eefd88552374e8958531bc9e51803f82b1738ee1 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 26 Sep 2024 14:08:18 -0600 Subject: [PATCH 20/20] AP_Scripting: Scripted follow in Plane --- libraries/AP_Scripting/applets/plane_follow.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index f676b3cc4cd76..004069845a234 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -818,7 +818,7 @@ local function update() --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f diff %.0f err %.3f ", xy_dist, projected_distance, distance, distance_error)) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": dst %.1f prj %.1f asp %.1f NEW %.1f fac %.1f ", xy_dist, projected_distance, desired_airspeed, airspeed_new, speed_factor)) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": tgt hdg:%.0f veh hdg: %.0f des hdg %.0f ang tar %.0f", target_heading, vehicle_heading, desired_heading, target_angle )) - gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target alt: %.1f desired alt %.1f frame %d", target_location:alt() * 0.01, target_altitude, foll_alt_type )) + --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": target alt: %.1f desired alt %.1f frame %d", target_location:alt() * 0.01, target_altitude, foll_alt_type )) --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(": distance %.0f y %.0f ", target_distance_offsets:length(), target_distance_offsets:y())) if math.abs(xy_dist) < wp_loiter_rad and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then --gcs:send_text(MAV_SEVERITY.NOTICE, SCRIPT_NAME_SHORT .. string.format(" REVERSE: distance %.1f offset_angle %.1f ", xy_dist, offset_angle ))