diff --git a/libraries/AP_Scripting/examples/plane-wind-fs.lua b/libraries/AP_Scripting/examples/plane-wind-fs.lua new file mode 100644 index 0000000000..b7ae5264dd --- /dev/null +++ b/libraries/AP_Scripting/examples/plane-wind-fs.lua @@ -0,0 +1,112 @@ +-- Adds a smart failsafe that accounts for how far the plane is from home +-- the average battery consumption, and the wind to decide when to failsafe +-- +-- CAUTION: This script only works for Plane + + +-- store the batt info as { instance, filtered, capacity } +local batt_info = { {0, 0, 0}, -- main battery + -- add more batteries to monitor here + } + +-- FIXME: check that the script is actually being run on ArduPilane + +local margin = 30 -- margin in seconds that should be added for the time to get home +local filter = 0.9 -- filter gain +local air_speed = 25.0 -- this should really be fetched from a parameter + +-- calculates the amount of time it will take for the vehicle to return home +-- returns 0 if there is no position, wind or home available +-- returns a negative number if it will take excessively long time, or is impossible +-- otherwise returns the time in seconds to get back +local function time_to_home() + local home = ahrs:get_home() + local position = ahrs:get_position() + local wind = ahrs:wind_estimate() + if home and position and wind then + local bearing = position:get_bearing(home) + local wind_bearing = math.atan(-wind:y(), -wind:x()) + local awa = math.abs(wind_bearing - bearing) -- acute wind angle + local ws = wind:length() -- wind speed (m/s) + local wca = math.asin(ws * math.sin(awa) / air_speed) -- wind correction angle + -- adjust the wind speed if it's pushing us the way we want to go + if awa > math.rad(90) then + ws = ws * -1 + end + -- compute the effective ground speed for getting home + local effective_speed = air_speed - (math.cos(wca) * ws) + -- clamp to prevent very large numbers + if effective_speed < 1.0 then + return -1 -- FIXME: this should really be infinity + end + return (position:get_distance(home) / effective_speed) + end + + return 0 -- nothing useful available +end + +local return_start + +-- this is an alternate update function that is simply used to track how long it will take to get home +-- it's really only used for debugging how the prediction rules are working +function track_return_time() + local home = ahrs:get_home() + local position = ahrs:get_position() + if home and position then + if position:get_distance(home) < 200 then + gcs:send_text(0, "Failsafe: RTL took " .. tostring((millis() - return_start) / 1000) .. " seconds") + return + end + end + return track_return_time, 100 +end + +-- the main update function that is used to decide when we should do a failsafe +function update() + for i = 1, #batt_info do + local instance, filtered_amps, rated_capacity_mah = table.unpack(batt_info[i]) + local amps = battery:current_amps(instance) + local consumed_mah = battery:consumed_mah(instance) + if amps and consumed_mah then + -- update all the current consumption rates + filtered_amps = (filtered_amps * filter) + (amps * (1.0 - filter)) + batt_info[i][2] = filtered_amps + + local remaining_capacity = (rated_capacity_mah - consumed_mah) * 3.6 -- amp seconds + local remaining_time = remaining_capacity / filtered_amps + local return_time = time_to_home() + if (return_time < 0) or (remaining_time < (return_time + margin)) then + -- FIXME: We need more insight into what the vehicles already doing. IE don't trigger RTL if we are already landing + gcs:send_text(0, string.format("Failsafe: Estimated %.0f seconds to home", return_time)) + vehicle:set_mode(11) -- plane RTL FIXME: we need a set of enums defined for the vehicles + -- swap to tracking the time rather then retrigger + return_start = millis() + return track_return_time, 100 + end + end + end + + return update, 100 +end + +-- validate that all the expected monitors have current monitoring capability, and fetch initial values +for i = 1, #batt_info do + -- check that the instance exsists + local instance = batt_info[i][1] + if instance > battery:num_instances() then + error("Battery " .. instance .. " does not exsist") + end + -- checkthat we can actually read current from the instance + if not battery:current_amps(instance) then + error("Battery " .. instance .. " does not support current monitoring") + end + -- store the pack capacity for later use, it's assumed to never change mid flight + local rated_cap = battery:pack_capacity_mah(instance) + if rated_cap then + batt_info[i][3] = rated_cap + else + error("Battery " .. instance .. " does not support current monitoring") + end +end + +return update, 100