diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 59e65dc343e738..a2ade0a6328506 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -963,6 +963,25 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const return g2.flight_options & flight_option; } +// This will only apply in Guided mode, other modes ignore it +// all the checks are in the Guided mode code, this is _desired_ speed +bool Plane::set_desired_speed(float speed) +{ + plane.guided_state.target_airspeed_cm = speed * 100.0f; + plane.guided_state.target_airspeed_time_ms = AP_HAL::millis(); + + plane.calc_airspeed_errors(); + + return true; +} + +// Helper function to let scripting set the guided mode radius +bool Plane::set_guided_radius_and_direction(float radius, bool direction_is_ccw) +{ + plane.mode_guided.set_radius_and_direction(radius, direction_is_ccw); + return true; +} + #if AC_PRECLAND_ENABLED void Plane::precland_update(void) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 613cc25a9b2480..76297ce92fd62d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1045,6 +1045,8 @@ class Plane : public AP_Vehicle { void update_flight_stage(); void set_flight_stage(AP_FixedWing::FlightStage fs); bool flight_option_enabled(FlightOptions flight_option) const; + bool set_desired_speed(float speed) override; + bool set_guided_radius_and_direction(float radius, bool direction_is_ccw) override; // navigation.cpp void loiter_angle_reset(void); diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 6a7896587114c7..f2d2c5795e61a9 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -203,6 +203,7 @@ void Plane::calc_airspeed_errors() target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100); } + //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "navigation guided - airspeed target cm: %d", target_airspeed_cm); #endif // OFFBOARD_GUIDED == ENABLED #if HAL_SOARING_ENABLED @@ -270,6 +271,8 @@ void Plane::calc_airspeed_errors() // Apply airspeed limit target_airspeed_cm = constrain_int32(target_airspeed_cm, aparm.airspeed_min*100, aparm.airspeed_max*100); + //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "navigation - airspeed target cm: %d", target_airspeed_cm); + // use the TECS view of the target airspeed for reporting, to take // account of the landing speed airspeed_error = TECS_controller.get_target_airspeed() - airspeed_measured; diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index a93f79d667ce21..d4b509a5d8314f 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -277,6 +277,7 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) { // exit immediately if not enabled if (!_enabled) { +// printf("follow not enabled\n"); return; } @@ -285,6 +286,9 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) return; } + //if (msg.sysid < 250) + // gcs().send_text(MAV_SEVERITY_INFO, "follow msg from: %d", msg.sysid); + // skip message if not from our target if (_sysid != 0 && msg.sysid != _sysid) { if (_automatic_sysid) { @@ -522,11 +526,13 @@ bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &ve bool AP_Follow::have_target(void) const { if (!_enabled) { +// gcs().send_text(MAV_SEVERITY_INFO, "AP_Follow: not enabled"); return false; } // check for timeout if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) { +// gcs().send_text(MAV_SEVERITY_INFO, "AP_Follow: timeout %d", _last_location_update_ms); return false; } return true; diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua new file mode 100644 index 00000000000000..601488a60bbdb8 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -0,0 +1,378 @@ +--[[ + support follow "mode" in plane. Theis will actually use GUIDED mode with + a scripting switch to allow guided to track + + This is a very simple implementation intended to act as a framework + for development of a custom solution +--]] + +local REFRESH_RATE = 0.1 -- in seconds, so 10Hz + +local PARAM_TABLE_KEY = 83 +local PARAM_TABLE_PREFIX = "FOLL_" + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local MODE_QLAND = 20 +local MODE_QLOITER = 19 +local MODE_GUIDED = 15 +local MODE_RTL = 11 + +local ALT_FRAME_ABSOLUTE = 0 + +local follow_enabled = false + +-- 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') + +-- All of the parameters here are modeled after Copter FOLLOW mode + +--[[ + // @Param: FOLL_SYSID + // @DisplayName: Plane Follow target mavlink system id + // @Description: The altitude (rangefinder distance) below which we stop using the precision landing sensor and continue landing + // @Range: 0 255 + // @User: Standard +--]] +--FOLL_SYSID = bind_add_param('SYSID', 1, 0) + +--[[ + // @Param: FOLL_DIST_MAX + // @DisplayName: Plane Follow maximum distance + // @Description: The distance from target beyond which the target is ignored + // @Range: 0 1000 + // @Units: m + // @User: Standard +--]] +--FOLL_DIST_MAX = bind_add_param('DIST_MAX', 2, 0) + +--[[ + // @Param: FOLL_OFS_X + // @DisplayName: Plane Follow offset in meters forward/backward + // @Description: Follow offsets in meters forward. Positive means fly ahead of the target, negative means fly behind. + // @Range: -100 100 + // @Units: m + // @User: Standard +--]] +--FOLL_OFS_X = bind_add_param('OFS_X', 3, 0 +FOLL_OFS_X = bind_param('FOLL_OFS_X') + +--[[ + // @Param: PFOLL_OFS_Y + // @DisplayName: Plane Follow offset in meters right/left + // @Description: Follow offsets in meters right/left. Positive means fly right of the target, negative means fly left. + // @Range: -100 100 + // @Units: m + // @User: Standard +--]] +--PFOLL_OFS_Y = bind_add_param('_OFS_Y', 4, 0) +FOLL_OFS_Y = bind_param('FOLL_OFS_Y') + +--[[ + // @Param: PFOLL_OFS_Z + // @DisplayName: Plane Follow offset in meters down/up + // @Description: Follow offsets in meters down. Positive means fly below of the target, negative means fly above. + // @Range: -100 100 + // @Units: m + // @User: Standard +--]] +--PFOLL_OFS_Z = bind_add_param('_OFS_Z', 5, 0) +FOLL_OFS_Z = bind_param('FOLL_OFS_Z') + +--[[ + // @Param: FOLL_POS_P + // @DisplayName: Plane Follow position error P gain + // @Description: Follow position error P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller + // @Range: 0.01 1 + // @User: Standard +--]] +--PFOLL_POS_P = bind_add_param('_POS_P', 6, 0) +FOLL_POS_P = bind_param('FOLL_POS_P') +FOLL_ALT_TYPE = bind_param('FOLL_ALT_TYPE') + +--[[ + // @Param: FOLL_FAIL_MODE + // @DisplayName: Plane Follow lost target mode + // @Description: Mode to switch to if the target is lost (no signal or > PFOLL_DIST_MAX). Defaults to RTL. + // @User: Standard +--]] +FOLL_FAIL_MODE = bind_add_param('FAIL_MODE', 7, 11) + +--[[ + // @Param: FOLL_EXIT_MODE + // @DisplayName: Plane Follow exit mode + // @Description: Mode to switch to when follow mode is exited. + // @User: Standard +--]] +FOLL_EXIT_MODE = bind_add_param('EXIT_MODE', 8, 11) + +--[[ + // @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) + + +--[[ + // @Param: FOLL_VEL_DZ + // @DisplayName: Plane Follow Deadzone + // @Description: Assume plane is "close enough" if less than this distance from the target. + // @Units: m + // @Range: 0 50 +--]] +FOLL_VEL_DZ = bind_add_param("VEL_DZ", 10, 10) + +last_follow_active_state = rc:get_aux_cached(FOLL_ACT_FN:get()) + +local have_target = false + +--[[ + import mavlink support for NAMED_VALUE_FLOAT, only used for + DUAL_AIRCRAFT operation + +local function mavlink_receiver() + local self = {} + local mavlink_msgs = require("mavlink_msgs") + local GLOBAL_POSITION_INT_msgid = mavlink_msgs.get_msgid("GOBAL_POSITION_INT") + local msg_map = {} + local jitter_correction = JitterCorrection(5000, 100) + + msg_map[NAMED_VALUE_FLOAT_msgid] = "NAMED_VALUE_FLOAT" + + -- initialise mavlink rx with number of messages, and buffer depth + mavlink.init(1, 10) + + -- register message id to receive + mavlink.register_rx_msgid(GLOBAL_POSITION_INT_msgid) + +--get a GLOBAL_POSITION_INT_msgid incoming message, but only from the target SYSID + function self.get_global_position_int() + local msg,_,timestamp_ms = mavlink.receive_chan() + if msg then + local parsed_msg = mavlink_msgs.decode(msg, msg_map) + if (parsed_msg ~= nil) and (parsed_msg.msgid == GLOBAL_POSITION_INT_msgid) then + if (PFOLL_SYSID:get() ~= nil) and parsed_msg.sysid == PFOLL_SYSID:get()) then + -- convert remote timestamp to local timestamp with jitter correction + local time_boot_ms = jitter_correction.correct_offboard_timestamp_msec(parsed_msg.time_boot_ms, timestamp_ms:toint()) + local value = parsed_msg.value + local name = bytes_to_string(parsed_msg.name) + return time_boot_ms, name, value, parsed_msg.sysid + end + end + end + return nil + end + + return self +end + +// handle a GLOBAL_POSITION_INT message +bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet) +{ + if (_target_sysid != msg_sysid) { + return false; + } + + _target_sysid_location.lat = packet.lat; + _target_sysid_location.lng = packet.lon; + // global_position_int.alt is *UP*, so is location. + _target_sysid_location.set_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE); + _target_sysid_location_set = true; + + return true; +} + +local mavlink_handler = nil +mavlink_handler = mavlink_receiver() + +--[[ + update the have_target variable +--]] +local function update_target() + if not precland:healthy() then + have_target = false + return + end + local ok = precland:target_acquired() + + if PFOLL_ALT_CUTOFF:get() > 0 then + -- require rangefinder as well + if not rangefinder:has_data_orient(rangefinder_orient) then + ok = false + end + end + + if ok ~= have_target then + have_target = ok + if have_target then + gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: Target Acquired") + else + gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: Target Lost") + end + end +end + +--[[ + 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 + -- support precision loiter under pilot control + if follow_enabled then + if follow:have_target() then + return true + else + gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: enabled but no target") + 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, "Plane Follow: disabled") + end + elseif (active_state == 2) then + -- Follow enabled - switch to guided mode + vehicle:set_mode(MODE_GUIDED) + -- Force Guided Mode to not loiter by setting a tiny loiter radius. + vehicle:set_guided_radius_and_direction(2, false) + follow_enabled = true + gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: enabled") + else + -- Don't know what to do with the 3rd switch position right now. + end + last_follow_active_state = active_state + end +end + +-- main update function +local function update() + follow_check() + if not follow_active() then + return + end + -- gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: Active") + local target_sysid = follow:get_target_sysid() + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Location: target sysid %d", target_sysid)) + + --[[ + 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 vehicle_location = ahrs:get_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 + --target_location, target_velocity = follow:get_target_location_and_velocity() + -- even worse - bot internally call get_target_lcoation_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_offsets = follow:get_target_location_and_velocity_ofs() + target_distance, target_offsets, target_velocity = follow:get_target_dist_and_vel_ned() + if target_distance == nil or target_offsets == nil or target_velocity == nil 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, "Plane Follow: lost target") + return + end + + -- will delete this debug later + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: xy_dist %f", xy_dist)) + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: target_distance:length() %f", target_distance:length())) + --gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Location: x %f y %f z %f", target_offsets:x(), target_offsets:y(), target_offsets:z())) + --gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Velocity: x %f y %f z %f", target_velocity:x(), target_velocity:y(), target_velocity:z())) + --gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: --------- ") + --gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: target velocity %f", target_airspeed)) + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: vehicle airspeed %f %f", vehicle_airspeed, xy_dist - vehicle_airspeed * REFRESH_RATE * 2)) + if target_location ~= nil and target_velocity ~= nil and target_offsets ~= nil then + local xy_dist = target_offsets:length() + local vehicle_airspeed = ahrs:airspeed_estimate() + local target_airspeed = target_velocity:length() + + -- first we tell the vehicle where we want to fly to. Guided mode figures how how to fly there. + -- my version + --local followok = vehicle:set_target_location(target_location) + -- plane_guided_follow version + local current_target = vehicle:get_target_location() + if not current_target then + return + end + + -- set the target frame as per user set parameter + local altitude_frame = FOLL_ALT_TYPE:get() + if altitude_frame == nil then + altitude_frame = 0 + end + target_location:change_alt_frame(altitude_frame) + -- update the target position from the follow library, which includes the offsets + vehicle:update_target_location(current_target, target_location) + + -- if the current velocity will not catch up to the target then we need to speed up use 12 seconds as a reasonable time to try to catch up + -- first figure out how far away we will be from the required location in 12 seconds if we maintain the current vehicle and target airspeeds + -- There is nothing magic about 12, it is just "what works" + local projected_distance = xy_dist - vehicle_airspeed * 12 + target_airspeed * 12 + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: projected distance %f", projected_distance)) + + -- now calculate what airspeed we will need to fly for that 12 seconds to catch up the projected distance + local desired_airspeed = projected_distance / 12 + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Plane Follow: desired velocity %f", desired_airspeed)) + vehicle:set_desired_speed(desired_airspeed) + -- the desired airspeed will never be acheived, don't worry -we will do this calculation again in REFRESH_RATE seconds, so we can adjust + end +end + +gcs:send_text(MAV_SEVERITY.INFO, "Plane Follow: Loaded") + +-- 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(0, "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 + +-- start running update loop +return protected_wrapper() + diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 6aa389f0cd5498..aa146a4cfc0c19 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2349,6 +2349,12 @@ function vehicle:nav_scripting_enable(param1) end ---@return boolean function vehicle:set_desired_speed(param1) end +-- desc +---@return boolean +---@param radius float +---@param direction_is_ccw boolean +function vehicle:set_guided_radius_and_direction() end + -- desc ---@param param1 number ---@param param2 number @@ -3300,6 +3306,12 @@ function follow:get_target_location_and_velocity_ofs() end ---@return Vector3f_ud|nil function follow:get_target_location_and_velocity() end +-- desc +---@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 @@ -3309,6 +3321,15 @@ function follow:get_last_update_ms() end function follow:have_target() end -- desc +---@return uint8_t_ud +function follow:get_target_sysid() end + +-- desc +---@return float +function follow:get_distance_to_target() end + +-- desc +---@class scripting scripting = {} -- desc diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 4a6d050874902d..e71c8e854856c8 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -337,6 +337,7 @@ singleton AP_Vehicle method set_target_throttle_rate_rpy void float -100 100 flo singleton AP_Vehicle method set_rudder_offset void float'skip_check boolean singleton AP_Vehicle method set_desired_turn_rate_and_speed boolean float'skip_check float'skip_check singleton AP_Vehicle method set_desired_speed boolean float'skip_check +singleton AP_Vehicle method set_guided_radius_and_direction boolean float'skip_check boolean singleton AP_Vehicle method nav_scripting_enable boolean uint8_t'skip_check singleton AP_Vehicle method set_velocity_match boolean Vector2f singleton AP_Vehicle method set_land_descent_rate boolean float'skip_check @@ -534,6 +535,10 @@ singleton QuadPlane method in_assisted_flight boolean singleton QuadPlane method abort_landing boolean singleton QuadPlane method in_vtol_land_descent boolean +include ../ArduPlane/mode.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane) +userdata ModeGuided rename modeguided +userdata ModeGuided method set_radius_and_direction void float'skip_check boolean + include ../ArduSub/Sub.h depends APM_BUILD_TYPE(APM_BUILD_ArduSub) singleton Sub rename sub singleton Sub depends APM_BUILD_TYPE(APM_BUILD_ArduSub) @@ -714,9 +719,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 diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 8b22c9395d73d4..a5f61f3da92e38 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -184,6 +184,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; } virtual bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; } virtual bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) { return false; } + virtual bool set_guided_radius_and_direction(float radius, bool direction_is_ccw) { return false; } // command throttle percentage and roll, pitch, yaw target // rates. For use with scripting controllers