Scripting: Failsafes: Change do_speed_change approach in short failsa…

…fe action

libraries/AP_Scripting/examples/failsafes/failsafes.lua

@@ -20,11 +20,11 @@ if not ICE_RPM_CHAN:init('ICE_RPM_CHAN') then
    gcs:send_text(6, 'get ICE_RPM_CHAN failed')
 end
 
--- need the sysid of this vehicle so we can send MAVLink commands to it
-local SYSID_THISMAV = Parameter()
-if not SYSID_THISMAV:init('SYSID_THISMAV') then
-   gcs:send_text(6, 'get SYSID_THISMAV failed')
+local TECS_SPDWEIGHT = Parameter()
+if not TECS_SPDWEIGHT:init('TECS_SPDWEIGHT') then
+   gcs:send_text(6, 'get TECS_SPDWEIGHT failed')
 end
+local _init_spdweight = TECS_SPDWEIGHT:get()
 
 -- Create param table for "Lua FailSafes"
 local PARAM_TABLE_KEY = 75 -- Does not clash with ready_take_off script table (key = 74)
@@ -59,75 +59,8 @@ _fs_reason_str[REASON_GPS_FS] = "GPS Lost"
 _fs_reason_str[REASON_GEOFENCE_FS] = "Fence Breach"
 _fs_reason_str[REASON_TELEM_FS] = "GCS Lost"
 
--- Mavlink setup for sending DO_CHANGE_SPEED commands
-local mavlink_msgs = require("MAVLink/mavlink_msgs")
 
-local COMMAND_ACK_ID = mavlink_msgs.get_msgid("COMMAND_ACK")
-local COMMAND_LONG_ID = mavlink_msgs.get_msgid("COMMAND_LONG")
-
-local msg_map = {}
-msg_map[COMMAND_ACK_ID] = "COMMAND_ACK"
-msg_map[COMMAND_LONG_ID] = "COMMAND_LONG"
-
--- initialize MAVLink rx with number of messages, and buffer depth
-mavlink.init(1, 10)
-
--- register message id to receive
-mavlink:register_rx_msgid(COMMAND_ACK_ID)
-
-
-local _confirm_counter = 0
 local _set_speed_complete = false
--- Send DO_CHANGE_SPEED mavlink command to bring the aircraft to the safe chute release speed
-function do_set_speed()
-   local MAV_CMD_DO_CHANGE_SPEED = 178 -- msg enum
-
-   local msg, chan = mavlink:receive_chan()
-
-   -- See if we got an ack
-   if (_confirm_counter > 0) and (msg ~= nil) then
-      local parsed_msg = mavlink_msgs.decode(msg, msg_map)
-      if (parsed_msg ~= nil) then
-         gcs:send_text(2, "DB: " .. tostring(parsed_msg.command))
-         if (parsed_msg.command == COMMAND_ACK_ID) then
-            gcs:send_text(2, "DB: Got cmd ACK")
-            _set_speed_complete = true
-         elseif (parsed_msg.command == MAV_CMD_DO_CHANGE_SPEED) then
-            gcs:send_text(2, "DB: Got change speed ACK")
-            _set_speed_complete = true
-         end
-      end
-   end
-
-   -- We do not need to send another change speed command if complete
-   if (_set_speed_complete) then
-      return
-   end
-
-   -- Build do change speed command and send
-   local cmd = {}
-   cmd.param1 = 0 -- speed type is airspeed
-   cmd.param2 = LFS_CHUTE_SPD:get() -- (m/s)
-   cmd.param3 = -2 -- default value
-   cmd.param4 = 0 -- not used
-   cmd.param5 = 0 -- not used
-   cmd.param6 = 0 -- not used
-   cmd.param7 = 0 -- not used
-   cmd.command = MAV_CMD_DO_CHANGE_SPEED
-   cmd.target_system = SYSID_THISMAV:get()
-   cmd.target_component = 0
-   cmd.confirmation = 0
-   mavlink:send_chan(0, mavlink_msgs.encode("COMMAND_LONG", cmd))
-
-   _confirm_counter = _confirm_counter + 1
-
-   if _confirm_counter >= 255 then
-      gcs:send_text(3, "DO_CHANGE_SPEED cmd failed to send")
-      _confirm_counter = 0
-   end
-end
-
-
 function do_fs_short_action(now_ms, reason)
    -- protection to prevent mode changes if we have already popped the chute
    if _in_failsafe_long then
@@ -147,7 +80,13 @@ function do_fs_short_action(now_ms, reason)
    end
 
    if _have_set_mode and not _set_speed_complete then
-      do_set_speed()
+      local airspeed_ms = LFS_CHUTE_SPD:get()
+      _set_speed_complete = vehicle:do_change_airspeed(airspeed_ms)
+
+      if (reason == REASON_RPM_FS) then
+         -- Change TECS_SPDWEIGHT to ensure we prioritise the safe flying airspeed with pitch control
+         TECS_SPDWEIGHT:set(2)
+      end
    end
 end
 
@@ -170,6 +109,7 @@ function reset_fs()
    _have_set_mode = false
    _confirm_counter = 0
    _set_speed_complete = false
+   TECS_SPDWEIGHT:set(_init_spdweight)
 end
 
 
@@ -225,6 +165,7 @@ function update()
    local reason = 0
    local reason_bm = 0
    if (in_rpm_fs) then
+      -- RPM reason should always be last as this can result in an engine failure and we want short action to modify speed weight for safe gliding
       reason = REASON_RPM_FS
       reason_bm = reason_bm | 1 << REASON_RPM_FS
    end