diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua new file mode 100644 index 0000000000000..0512fca6acc83 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -0,0 +1,808 @@ +--[[ + + 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 + 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-043" +SCRIPT_NAME = "Plane Follow" +SCRIPT_NAME_SHORT = "PFollow" + +-- 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, 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, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points + +FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +local ahrs_eas2tas = ahrs:get_EAS2TAS() +local windspeed_vector = ahrs:wind_estimate() + +local now = millis():tofloat() * 0.001 +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_heading1 = -400.0 +local save_target_heading2 = -400.0 +local save_target_altitude +local tight_turn = false + +local PARAM_TABLE_KEY = 120 +local PARAM_TABLE_PREFIX = "ZPF_" +local PARAM_TABLE_KEY2 = 121 +local PARAM_TABLE_PREFIX2 = "ZPF2_" + +-- 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 Parameter(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') + +-- add a parameter and bind it to a variable +local function bind_add_param2(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX2 .. name) +end +-- setup follow mode specific parameters- second tranche +assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 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 + +-- we need these existing FOLL_ parametrs +FOLL_ALT_TYPE = Parameter('FOLL_ALT_TYPE') +FOLL_SYSID = Parameter('FOLL_SYSID') +FOLL_TIMEOUT = Parameter('FOLL_TIMEOUT') +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.0 +local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL +local foll_timeout = FOLL_TIMEOUT:get() or 1000 + +-- Add these ZPF_ parameters specifically for this script +--[[ + // @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 +--]] +ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.LOITER) + +--[[ + // @Param: ZPF_EXIT_MODE + // @DisplayName: Plane Follow exit mode + // @Description: Mode to switch to when follow mode is exited normally + // @User: Standard +--]] +ZPF_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER) + +--[[ + // @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 +--]] +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: s +--]] +ZPF_TIMEOUT = bind_add_param("TIMEOUT", 4, 10) + +--[[ + // @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: deg +--]] +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: deg +--]] +ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 15) + +--[[ + // @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: m +--]] +ZPF_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 50) + +--[[ + // @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 +--]] +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: m +--]] +ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) + +--[[ + // @Param: ZPF_D_P + // @DisplayName: Plane Follow Scripting distance P gain + // @Description: P gain for the speed PID controller distance component + // @Range: 0 1 +--]] +ZPF2_D_P = bind_add_param2("D_P", 1, 0.3) + +--[[ + // @Param: ZPF_D_I + // @DisplayName: Plane Follow Scripting distance I gain + // @Description: I gain for the speed PID distance component + // @Range: 0 1 +--]] +ZPF2_D_I = bind_add_param2("D_I", 2, 0.3) + +--[[ + // @Param: ZPF_D_D + // @DisplayName: Plane Follow Scripting distance D gain + // @Description: D gain for the speed PID controller distance component + // @Range: 0 1 +--]] +ZPF2_D_D = bind_add_param2("D_D", 3, 0.05) + +--[[ + // @Param: ZPF_V_P + // @DisplayName: Plane Follow Scripting speed P gain + // @Description: P gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF2_V_P = bind_add_param2("V_P", 4, 0.3) + +--[[ + // @Param: ZPF_V_I + // @DisplayName: Plane Follow Scripting speed I gain + // @Description: I gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF2_V_I = bind_add_param2("V_I", 5, 0.3) + +--[[ + // @Param: ZPF_V_D + // @DisplayName: Plane Follow Scripting speed D gain + // @Description: D gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF2_V_D = bind_add_param2("V_D", 6, 0.05) + +--[[ + // @Param: ZPF_LkAHD + // @DisplayName: Plane Follow Lookahead seconds + // @Description: Time to "lookahead" when calculating distance errors + // @Units: s +--]] +ZPF2_LKAHD = bind_add_param2("LKAHD", 7, 5) + +--[[ + // @Param: ZPF_DIST_FUDGE + // @DisplayName: Plane Follow distance fudge factor + // @Description: THe distance returned by the AP_FOLLOW library might be off by about this factor of airspeed + // @Units: s +--]] +ZPF2_DIST_FUDGE = bind_add_param2("DIST_FUDGE", 8, 0.92) + +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 + +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 distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92 + +DISTANCE_LOOKAHEAD_SECONDS = ZPF2_LKAHD:get() or 5.0 + +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 + +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +local speedpid = require("speedpid") +local pid_controller_distance = speedpid.speed_controller(ZPF2_D_P:get() or 0.3, + ZPF2_D_I:get() or 0.3, + ZPF2_D_D:get() or 0.05, + 5.0, airspeed_min - airspeed_max, airspeed_max - airspeed_min) + +local pid_controller_velocity = speedpid.speed_controller(ZPF2_V_P:get() or 0.3, + ZPF2_V_I:get() or 0.3, + ZPF2_V_D:get() or 0.05, + 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; + 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 + +-- 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.speed = z speed of change to altitude (1000.0 = max) +local function set_vehicle_target_altitude(target) + 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 + end + -- 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 = speed, + z = target.alt }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") + end +end + +-- 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 heading_heading == nil or heading_heading <= -400 or heading_heading > 360 then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_heading no heading") + return + end + + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL, + 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 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 + 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 radius = target.radius or 2.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) + -- yaw > 0 - CCW = turn to the right of the target point + -- yaw < 0 - Clockwise = turn to the left of the target point + -- if following direct we turn on the "outside" + + -- 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(ZPF_ACT_FN:get()) + +--[[ + 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 == FLIGHT_MODE.GUIDED then + if follow_enabled then + if follow:have_target() then + reported_target = true + else + if reported_target then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid()) + end + reported_target = false + end + end + else + reported_target = false + end + + return reported_target +end + +--[[ + check for user activating follow using an RC switch set HIGH +--]] +local function follow_check() + if ZPF_ACT_FN == nil then + return + end + local foll_act_fn = ZPF_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 + -- Follow disabled - return to EXIT mode + vehicle:set_mode(exit_mode) + 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(FLIGHT_MODE.GUIDED) + follow_enabled = true + lost_target_countdown = LOST_TARGET_TIMEOUT + --speed_controller_pid.reset() + pid_controller_distance.reset() + pid_controller_velocity.reset() + 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 + +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 function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +local function calculate_airspeed_from_groundspeed(velocity_vector) + --[[ + 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 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 + + return airspeed +end + +-- main update function +local function update() + now = millis():tofloat() * 0.001 + ahrs_eas2tas = ahrs:get_EAS2TAS() + windspeed_vector = ahrs:wind_estimate() + + follow_check() + if not follow_active() then + return + end + + -- set the target frame as per user set parameter - this is fundamental to this working correctly + local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 + local long_distance = close_distance * 4.0 + 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.0 + foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL + use_wide_turns = ZPF_WIDE_TURNS:get() or 1 + distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92 + + --[[ + get the current navigation target. + --]] + local target_location -- = Location() of the target + 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_distance_offset -- vector to the target taking offsets into account + local xy_dist -- distance to target with offsets in meters + local target_heading -- heading of the target vehicle + + local current_location = ahrs:get_location() + if current_location == nil then + return + end + ---@cast current_location -nil + local current_altitude = current_location:alt() * 0.01 + + 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 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_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_offset, + target_velocity, target_velocity_offset, + target_location, target_location_offset, + xy_dist = follow:get_target_info() + target_heading = follow:get_target_heading_deg() or -400 + + -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to reaquire it + if target_location == nil or target_location_offset == nil or + target_velocity == nil or target_velocity_offset == nil or + target_distance_offset == nil or current_target == nil or target_distance == nil or xy_dist == nil then + lost_target_countdown = lost_target_countdown - 1 + if lost_target_countdown <= 0 then + follow_enabled = false + vehicle:set_mode(fail_mode) + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") + return + end + + -- maintain the current heading until we re-establish telemetry from the target vehicle -- note that this is not logged, needs fixing + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: lost " .. FOLL_SYSID:get() .. " FAILED hdg: " .. save_target_heading1) + set_vehicle_heading({heading = save_target_heading1}) + set_vehicle_target_altitude({alt = save_target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + 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 + local target_airspeed = calculate_airspeed_from_groundspeed(target_velocity_offset) + + local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw()))) + local heading_to_target_offset = math.deg(current_location:get_bearing(target_location_offset)) + + -- offset_angle is the difference between the current heading of the follow vehicle and the target_location (with offsets) + local offset_angle = wrap_180(vehicle_heading - heading_to_target_offset) + + -- rotate the target_distance_offsets in NED to the same direction has the follow vehicle, we use this below + local target_distance_rotated = target_distance_offset:copy() + target_distance_rotated:rotate_xy(math.rad(vehicle_heading)) + + -- default the desired heading to the target heading (adjusted for projected turns) - we might change this below + local airspeed_difference = vehicle_airspeed - target_airspeed + + -- distance seem to be out by about 0.92s at approximately current airspeed just eyeballing it. + xy_dist = math.abs(xy_dist) - vehicle_airspeed * distance_fudge + -- 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) < long_distance) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then + xy_dist = -xy_dist + end + + -- 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 + + -- 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 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)) + if angle_diff2 > angle_diff1 then + target_angle = angle_diff2 + else + target_angle = angle_diff1 + 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_altitude = current_altitude + save_target_heading2 = save_target_heading1 + save_target_heading1 = target_heading + now_target_heading = now + end + end + + -- 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 turning = (math.abs(projected_distance) < long_distance and math.abs(target_angle) > TURNING_ANGLE) + local turn_starting = false + local target_attitude = mavlink_attitude_receiver.get_attitude(foll_sysid) + local pre_roll_target_heading = target_heading + local desired_heading = target_heading + local angle_adjustment + tight_turn = false + if target_attitude ~= nil then + if (now - target_attitude.timestamp_ms < foll_timeout) and + 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 + target_attitude.rollspeed)) + + turning = true + -- predict the roll in 1s from now and use that based on rollspeed + -- need some more maths to convert a roll angle into a turn angle - from Mission Planner: + -- 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)) + + angle_adjustment = tangent_angle * 0.6 + -- if the roll direction is the same as the y offset, then we are turning on the "inside" (a tight turn) + if (target_attitude.roll < 0 and foll_ofs_y < 0) or + (target_attitude.roll > 0 and foll_ofs_y > 0) then + tight_turn = true + end + + -- if the roll direction is the same as the rollspeed then we are heading into a turn, otherwise we are finishing a turn + if foll_ofs_y == 0 or + (target_attitude.roll < 0 and target_attitude.rollspeed < 0) or + (target_attitude.roll > 0 and target_attitude.rollspeed > 0) then + turn_starting = true + target_angle = wrap_360(target_angle - angle_adjustment) + desired_heading = wrap_360(target_heading - angle_adjustment) + -- push the target heading back because it hasn't figured out we are turning yet + save_target_heading1 = save_target_heading2 + end + end + end + + -- 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) + + -- 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 or 0.0) < close_distance) + or offset_angle > OVERSHOOT_ANGLE + ) + + local distance_error = constrain(math.abs(projected_distance) / (close_distance * DISTANCE_LOOKAHEAD_SECONDS), 0.1, 1.0) + if overshot or too_close or (too_close_follow_up > 0) then + if too_close_follow_up > 0 then + too_close = true + too_close_follow_up = too_close_follow_up - 1 + else + too_close_follow_up = 10 + end + else + too_close_follow_up = 0 + end + + local target_altitude = 0.0 + local frame_type_log = foll_alt_type + + if altitude_override ~= 0 then + target_altitude = altitude_override + frame_type_log = -1 + elseif target_location_offset ~= nil then + -- 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 + + local mechanism = 0 -- for logging 1: position/location 2:heading + local normalized_distance = math.abs(projected_distance) + local close = (normalized_distance < close_distance) + local too_wide = (math.abs(target_distance_rotated:y()) > (close_distance/5) and not turning) + + -- xy_dist < 3.0 is a special case because mode_guided will try to loiter around the target location if within 2m + -- target_heading - vehicle_heading catches the circumstance where the target vehicle is heaidng in completely the opposite direction + if math.abs(xy_dist or 0.0) < 3.0 or + ((turning and ((tight_turn and turn_starting) or use_wide_turns or foll_ofs_y == 0)) or -- turning + ((close or overshot) and not too_wide) -- we are very close to the target + --math.abs(target_heading - vehicle_heading) > 135) -- the target is going the other way + ) 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 + elseif target_location_offset ~= nil then + set_vehicle_target_location({lat = target_location_offset:lat(), + lng = target_location_offset:lng(), + alt = target_altitude, + frame = foll_alt_type, + yaw = foll_ofs_y}) + mechanism = 1 -- position/location - for logging + end + -- dv = interim delta velocity based on the pid controller using projected_distance as the error (we want distance == 0) + local dv = pid_controller_distance.update(target_airspeed - vehicle_airspeed, projected_distance) + local airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv) + + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + + 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','mmmnnn----','----------', + xy_dist, + projected_distance, + distance_error, + target_airspeed, + vehicle_airspeed, + airspeed_new, + mechanism, log_too_close, log_too_close_follow_up, log_overshot + ) + logger:write("ZPF2",'AngT,AngO,Alt,AltT,AltFrm,HdgT,Hdg,HdgP,HdgO','ffffbffff','ddmm-dddd','---------', + target_angle, + offset_angle, + current_altitude, + target_altitude, + frame_type_log, + target_heading, + vehicle_heading, + pre_roll_target_heading, + desired_heading + ) +end + +local banner_displayed = false +-- wrapper around update(). This calls update() at 1/REFRESH_RATE Hz +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + + if not banner_displayed then + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + banner_displayed = true + end + + 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 + +-- start running update loop - waiting 30s for the AP to initialize +return protected_wrapper, 20000 + diff --git a/libraries/AP_Scripting/applets/plane_follow.md b/libraries/AP_Scripting/applets/plane_follow.md new file mode 100644 index 0000000000000..0ffb96ad2cd20 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.md @@ -0,0 +1,129 @@ +# Plane Follow + +This script implements follow functionality for Plane. The plane must be +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 + +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. In addition +the following "ZPF" parameters are added: Z = scripting, P = Plane, F = Follow, in two +banks ZPF and ZPF2. + +## ZPF_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_MAX. + +## ZPF_EXIT_MODE + +The flight mode the plane will switch to if it exits following. + +## ZPF_ACT_FN + +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. + +## ZPF_TIMEOUT + +If the target is lost, this is the timeout to wait to re-aquire the target before +triggering ZPF_FAIL_MODE + +## ZPF_OVRSHT_DEG + +This is for the heuristic that uses the difference between the target vehicle heading +and the follow vehicle heading to determine if the vehicle has overshot and should slow +down and turn around. 75 degrees is a good start but tune for your circumstances. + +## ZPF_TURN_DEG + +This is for the heuristic that uses the difference between the target vehicle heading +and the follow vehicle heading to determine if the target vehicle is executing a turn. +15 degrees is a good start but tune for your circumstances. + +## ZPF_DIST_CLOSE + +One of the most important heuristics the follow logic uses to match the heading and speed +of the target plane is to trigger different behavior when the target location is "close". +How close is determined by this value, likely a larger number makes more sense for larger +and faster vehicles and lower values for smaller and slower vehicles. Tune for your circumstances. + +## ZPF_ALT_OVR + +The follow logic can have the follow vehicle track the altitude of the target, but setting a value +in ZPF_ALT_OVR allows the follow vehicle to follow at a fixed altitude regardless of the altitude +of the target. The ZPF_ALT_OVR is in meters in FOLL_ALT_TYPE frame. + +## ZPF2_D_P + +The follow logic uses two PID controllers for controlling speed, the first uses distance (D) +as the error. This is the P gain for the "D" PID controller. + +## ZPF2_D_I + +The follow logic uses two PID controllers for controlling speed, the first uses distance (D) +as the error. This is the I gain for the "D" PID controller. + +## ZPF2_D_D + +The follow logic uses two PID controllers for controlling speed, the first uses distance (D) +as the error. This is the D gain for the "D" PID controller. + +## ZPF2_V_P + +The follow logic uses two PID controllers for controlling speed, the first uses velocity (V) +as the error. This is the P gain for the "V" PID controller. + +## ZPF2_V_I + +The follow logic uses two PID controllers for controlling speed, the first uses distance (V) +as the error. This is the I gain for the "V" PID controller. + +## ZPF2_V_D + +The follow logic uses two PID controllers for controlling speed, the first uses distance (V) +as the error. This is the D gain for the "V" PID controller. + +## ZPF2_LKAHD + +Time to "lookahead" when calculating distance errors. + +## ZPF2_DIST_FUDGE + +This parameter might be a bad idea, but it seems the xy_distance between the target offset location +and the follow vehicle returned by AP_Follow appears to be off by a factor of +airspeed * a fudge factor +This allows this fudge factor to be adjusted until a better solution can be found for this problem. + +# 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. + +Install the speedpid.lua, mavlink_attitude.lua and mavlink_msgs.lua files +in the APM/scripts/modules directory on the SD card. + +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. + +The target location the plane will attempt to achieve 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. ZPF2_ALT_OVR will override the operation of FOLL_OFS_Z +setting a fixed altitude for the following plane in FOLL_ALT_TYPE frame. + +To ensure the follow plane gets timely updates from the target, the SRx_EXT_STAT and SRx_EXTRA1 +telemetry stream rate parameters should be increased to increase the rate that the POSITION_TARGET_GLOBAL_INT +and ATTITUDE mavlink messages are sent. The default value is 4Hz, a good value is probably 8Hz or 10Hz but +some testing should be done to confirm the best rate for your telemetry radios and vehicles. diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 3a34534316de8..c6880d718ff09 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -414,7 +414,6 @@ function CAN:get_device(buffer_len) end ---@return ScriptingCANBuffer_ud|nil function CAN:get_device2(buffer_len) end - -- get latest FlexDebug message from a CAN node ---@param bus number -- CAN bus number, 0 for first bus, 1 for 2nd ---@param node number -- CAN node @@ -968,6 +967,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 @@ -1183,6 +1186,12 @@ 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 number -- 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 = {} @@ -3786,20 +3795,28 @@ function precland:healthy() end -- desc follow = {} --- desc +-- true if we have a valid target location estimate +---@return boolean +function follow:have_target() end + +-- get the SYSID_THISMAV of the target +---@return uint32_t_ud +function follow:get_target_sysid() end + +-- get target's heading in degrees (0 = north, 90 = east) ---@return number|nil function follow:get_target_heading_deg() end --- desc ----@return Location_ud|nil ----@return Vector3f_ud|nil -function follow:get_target_location_and_velocity_ofs() 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 uint32_t_ud function follow:get_last_update_ms() end @@ -3808,6 +3825,16 @@ 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 -- dist_ned - distance to the target +---@return Vector3f_ud|nil -- dist_with_offs - distance to the target with offsets +---@return Vector3f_ud|nil -- target_vel_ned - proposed velocity of the target +---@return Vector3f_ud|nil -- target_vel_ned_ofs - proposed velocity of the target with offsets +---@return Location_ud|nil -- target_loc - location of the target +---@return Location_ud|nil -- target_loc_ofs - location of the target with offsets +---@return number|nil -- target_dist_ofs - distance to the target in meters +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 ae6ba0a48f426..1aef9e4af86fb 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -284,7 +284,7 @@ singleton AP_Relay method toggle void uint8_t 0 AP_RELAY_NUM_RELAYS singleton AP_Relay method get uint8_t uint8_t 0 AP_RELAY_NUM_RELAYS include GCS_MAVLink/GCS.h -singleton GCS depends (HAL_GCS_ENABLED && !defined(HAL_BUILD_AP_PERIPH)) +singleton GCS depends HAL_GCS_ENABLED singleton GCS rename gcs singleton GCS method send_text void MAV_SEVERITY'enum MAV_SEVERITY_EMERGENCY MAV_SEVERITY_DEBUG "%s"'literal string singleton GCS method set_message_interval MAV_RESULT'enum uint8_t 0 MAVLINK_COMM_NUM_BUFFERS uint32_t'skip_check int32_t -1 INT32_MAX @@ -698,9 +698,6 @@ ap_object ScriptingCANBuffer method write_frame boolean AP_HAL::CANFrame uint32_ ap_object ScriptingCANBuffer method read_frame boolean AP_HAL::CANFrame'Null ap_object ScriptingCANBuffer method add_filter boolean uint32_t'skip_check uint32_t'skip_check -include AP_DroneCAN/AP_DroneCAN.h -global manual DroneCAN_get_FlexDebug lua_DroneCAN_get_FlexDebug 4 2 depends (HAL_ENABLE_DRONECAN_DRIVERS==1) - include ../Tools/AP_Periph/AP_Periph.h depends defined(HAL_BUILD_AP_PERIPH) singleton AP_Periph_FW depends defined(HAL_BUILD_AP_PERIPH) singleton AP_Periph_FW rename periph @@ -746,10 +743,13 @@ 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_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_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 +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) @@ -973,7 +973,7 @@ global manual dirlist lua_dirlist 1 2 global manual remove lua_removefile 1 3 global manual print lua_print 1 0 -singleton mavlink depends (HAL_GCS_ENABLED && !defined(HAL_BUILD_AP_PERIPH)) +singleton mavlink depends HAL_GCS_ENABLED singleton mavlink manual init lua_mavlink_init 2 0 singleton mavlink manual register_rx_msgid lua_mavlink_register_rx_msgid 1 1 singleton mavlink manual send_chan lua_mavlink_send_chan 3 1 diff --git a/libraries/AP_Scripting/modules/mavlink_attitude.lua b/libraries/AP_Scripting/modules/mavlink_attitude.lua new file mode 100644 index 0000000000000..a563698835131 --- /dev/null +++ b/libraries/AP_Scripting/modules/mavlink_attitude.lua @@ -0,0 +1,168 @@ +--[[ + + 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 . + + MAVLinkAttitude + A MAVlink message receiver for ATTITUDE messages specifically +--]] + +local MAVLinkAttitude = {} + +MAVLinkAttitude.SCRIPT_VERSION = "4.6.0-004" +MAVLinkAttitude.SCRIPT_NAME = "MAVLink Attitude" +MAVLinkAttitude.SCRIPT_NAME_SHORT = "MAVATT" + +--[[ + import mavlink support for NAMED_VALUE_FLOAT, only used for + DUAL_AIRCRAFT operation +--]] + +ATTITUDE_MESSAGE = "ATTITUDE" + +--[[ + a lua implementation of the jitter correction algorithm from libraries/AP_RTC + + note that the use of a 32 bit float lua number for a uint32_t + milliseconds means we lose accuracy over time. At 9 hours we have + an accuracy of about 1 millisecond +--]] +function MAVLinkAttitude.JitterCorrection(_max_lag_ms, _convergence_loops) + local self = {} + + local max_lag_ms = _max_lag_ms + local convergence_loops = _convergence_loops + local link_offset_ms = 0 + local min_sample_ms = 0 + local initialised = false + local min_sample_counter = 0 + + function self.correct_offboard_timestamp_msec(offboard_ms, local_ms) + local diff_ms = local_ms - offboard_ms + if not initialised or diff_ms < link_offset_ms then + --[[ + this message arrived from the remote system with a + timestamp that would imply the message was from the + future. We know that isn't possible, so we adjust down the + correction value + --]] + link_offset_ms = diff_ms + initialised = true + end + + local estimate_ms = offboard_ms + link_offset_ms + + if estimate_ms + max_lag_ms < local_ms then + --[[ + this implies the message came from too far in the past. clamp the lag estimate + to assume the message had maximum lag + --]] + estimate_ms = local_ms - max_lag_ms + link_offset_ms = estimate_ms - offboard_ms + end + + if min_sample_counter == 0 then + min_sample_ms = diff_ms + end + + min_sample_counter = (min_sample_counter+1) + if diff_ms < min_sample_ms then + min_sample_ms = diff_ms + end + if min_sample_counter == convergence_loops then + --[[ + we have the requested number of samples of the transport + lag for convergence. To account for long term clock drift + we set the diff we will use in future to this value + --]] + link_offset_ms = min_sample_ms + min_sample_counter = 0 + end + return estimate_ms + end + return self + end + +function MAVLinkAttitude.mavlink_attitude_receiver() + local self = {} + local ATTITUDE_map = {} + ATTITUDE_map.id = 30 + ATTITUDE_map.fields = { + { "time_boot_ms", ". + + 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-003" +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 + 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/modules/speedpid.lua b/libraries/AP_Scripting/modules/speedpid.lua new file mode 100644 index 0000000000000..708818ec8114c --- /dev/null +++ b/libraries/AP_Scripting/modules/speedpid.lua @@ -0,0 +1,213 @@ +--[[ + + 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 gains + 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 SpeedPID = {} + +SpeedPID.SCRIPT_VERSION = "4.6.0-003" +SpeedPID.SCRIPT_NAME = "Speed PID Controller" +SpeedPID.SCRIPT_NAME_SHORT = "SpeedPID" + +-- constrain a value between limits +function SpeedPID.constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +function SpeedPID.is_zero(x) + return math.abs(x) < 1.1920928955078125e-7 +end + +function SpeedPID.PID_controller(kP,kI,kD,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 _kD = kD or 0.0 + local _iMax = iMax + local _min = min + local _max = max + local _last_t = nil + local _error = 0.0 + local _derivative = 0.0 + local _reset_filter = true + local _filt_E_hz = 0.01 + local _filt_D_hz = 0.005 + local _D = 0 + local _I = 0 + local _P = 0 + local _total = 0 + local _counter = 0 + local _target = 0 + local _current = 0 + local nowPI = millis():tofloat() * 0.001 + + function self.calc_lowpass_alpha_dt(dt, cutoff_freq) + if (dt <= 0.0 or cutoff_freq <= 0.0) then + --INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + return 1.0 + end + if (SpeedPID.is_zero(cutoff_freq)) then + return 1.0 + end + if (SpeedPID.is_zero(dt)) then + return 0.0 + end + local rc = 1.0 / (math.pi * 2.0 * cutoff_freq) + return dt / (dt + rc); + end + + function self.get_filt_E_alpha(dt) + return self.calc_lowpass_alpha_dt(dt, _filt_E_hz); + end + + function self.get_filt_D_alpha(dt) + return self.calc_lowpass_alpha_dt(dt, _filt_D_hz); + end + + -- update the controller. + function self.update(target, current) + local now = micros() + if not _last_t then + _last_t = now + end + local dt = (now - _last_t):tofloat() * 0.000001 + _last_t = now + local err = target - current + _counter = _counter + 1 + + -- reset input filter to value received + if (_reset_filter) then + _reset_filter = false + _error = _target - current + _derivative = 0.0 + else + local error_last = _error; + _error = _error + self.get_filt_E_alpha(dt) * ((_target - current) - _error); + + -- calculate and filter derivative + if (dt >= 0) then + local derivative = (_error - error_last) / dt; + _derivative = _derivative + self.get_filt_D_alpha(dt) * (derivative - _derivative); + end + end + local D = _derivative * _kD + _D = D + + 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 = SpeedPID.constrain(_I, -_iMax, iMax) + end + local I = _I + local ret = target + P + I + D + if math.floor(now:tofloat() * 0.000001) ~= math.floor(nowPI) then + nowPI = micros():tofloat() * 0.000001 + end + _target = target + _current = current + _P = P + + ret = SpeedPID.constrain(ret, _min, _max) + _total = ret + return ret + end + + -- reset integrator to an initial value + function self.reset(integrator) + _I = integrator + _reset_filter = true + end + + function self.set_D(D) + _D = D + _D_error = 0 + 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,D,Total,Add','fffffff',_target,_current,_P,_I,_D,_total,add_total) + end + + -- return the instance + return self + end + +function SpeedPID.speed_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax) + local self = {} + local speedpid = SpeedPID.PID_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax) + + function self.update(spd_current, spd_error) + local adjustment = speedpid.update(spd_current + spd_error, spd_current) + speedpid.log("ZSPI", 0) -- Z = scripted, S = speed, PI = PI controller + return adjustment + end + + function self.reset() + speedpid.reset(0) + end + + return self +end + +gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", SpeedPID.SCRIPT_NAME, SpeedPID.SCRIPT_VERSION) ) + +return SpeedPID