AP_Scripting: add follow-target-send applet

libraries/AP_Scripting/applets/follow-target-send.lua

@@ -0,0 +1,133 @@
+-- Send the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one
+--
+-- How To Use
+-- 1. copy this script to the autopilot's "scripts" directory
+-- 2. within the "scripts" directory create a "modules" directory
+-- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory
+-- 4. the FOLLOW_TARGET message will be published at 10hz
+
+-- load mavlink message definitions from modules/MAVLink directory
+local mavlink_msgs = require("MAVLink/mavlink_msgs")
+
+-- global definitions
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+local UPDATE_INTERVAL_MS = 100                  -- update at about 10hz
+local FOLLOW_TARGET_CAPABILITIES = {POS=2^0, VEL=2^1, ACCEL=2^2, ATT_RATES=2^3}
+
+ -- setup script specific parameters
+local PARAM_TABLE_KEY = 88
+local PARAM_TABLE_PREFIX = "FOLT_"
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table')
+
+-- 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', PARAM_TABLE_PREFIX .. name))
+    return Parameter(PARAM_TABLE_PREFIX .. name)
+end
+
+--[[
+  // @Param: FOLT_ENABLE
+  // @DisplayName: Follow Target Send Enable
+  // @Description: Follow Target Send Enable
+  // @Values: 0:Disabled,1:Enabled
+  // @User: Standard
+--]]
+local FOLT_ENABLE = bind_add_param("ENABLE", 1, 1)
+
+--[[
+  // @Param: FOLT_MAV_CHAN
+  // @DisplayName: Follow Target Send MAVLink Channel
+  // @Description: MAVLink channel to which FOLLOW_TARGET should be sent
+  // @Range: 0 10
+  // @User: Standard
+--]]
+local FOLT_MAV_CHAN = bind_add_param("MAV_CHAN", 2, 0)
+
+-- initialize MAVLink rx with number of messages, and buffer depth
+mavlink:init(2, 5)
+
+-- send FOLLOW_TARGET message
+local function send_follow_target_msg()
+
+    -- get vehicle location
+    local curr_loc = ahrs:get_location()
+    if curr_loc == nil then
+        do return end
+    end
+
+    -- get vehicle target velocity in m/s in NED frame
+    local vel_target_NED = poscontrol:get_vel_target()
+    if vel_target_NED == nil then
+        vel_target_NED = Vector3f()
+    end
+
+    -- get vehicle target acceleration in m/s/s in NED frame
+    local accel_target_NED = poscontrol:get_accel_target()
+    if accel_target_NED == nil then
+        accel_target_NED = Vector3f()
+    end
+
+    -- get vehicle current attitude as quaternion
+    local attitude_quat = ahrs:get_quaternion()
+    if attitude_quat == nil then
+        attitude_quat = Quaternion()
+    end
+
+    -- get vehicle rotation rates
+    local curr_rot_rate = ahrs:get_gyro()
+    if curr_rot_rate == nil then
+        curr_rot_rate = Vector3f()
+    end
+    local curr_rot_rate_NED = ahrs:body_to_earth(curr_rot_rate)
+
+    -- prepare FOLLOW_TARGET message
+    local follow_target_msg = {}
+    follow_target_msg.timestamp = millis():toint() * 1000
+    follow_target_msg.est_capabilities = FOLLOW_TARGET_CAPABILITIES.POS + FOLLOW_TARGET_CAPABILITIES.VEL +
+                                         FOLLOW_TARGET_CAPABILITIES.ACCEL + FOLLOW_TARGET_CAPABILITIES.ATT_RATES
+    follow_target_msg.lat = curr_loc:lat()
+    follow_target_msg.lon = curr_loc:lng()
+    follow_target_msg.alt = curr_loc:alt() * 0.01
+    follow_target_msg.vel = {vel_target_NED:x(), vel_target_NED:y(), vel_target_NED:z()}
+    follow_target_msg.acc = {accel_target_NED:x(), accel_target_NED:y(), accel_target_NED:z()}
+    follow_target_msg.attitude_q = {attitude_quat:q1(), attitude_quat:q2(), attitude_quat:q3(), attitude_quat:q4()}
+    follow_target_msg.rates = {curr_rot_rate_NED:x(), curr_rot_rate_NED:y(), curr_rot_rate_NED:z()}
+    follow_target_msg.position_cov = {0, 0, 0}
+    follow_target_msg.custom_state = 0
+
+    -- send FOLLOW_TARGET message
+    mavlink:send_chan(FOLT_MAV_CHAN:get(), mavlink_msgs.encode("FOLLOW_TARGET", follow_target_msg))
+
+--[[
+    <field type="uint64_t" name="timestamp" units="ms">Timestamp (time since system boot).</field>
+    <field type="uint8_t" name="est_capabilities">bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = 2, ATT + RATES = 3)</field>
+    <field type="int32_t" name="lat" units="degE7">Latitude (WGS84)</field>
+    <field type="int32_t" name="lon" units="degE7">Longitude (WGS84)</field>
+    <field type="float" name="alt" units="m">Altitude (MSL)</field>
+    <field type="float[3]" name="vel" units="m/s">target velocity (0,0,0) for unknown</field>
+    <field type="float[3]" name="acc" units="m/s/s">linear target acceleration (0,0,0) for unknown</field>
+    <field type="float[4]" name="attitude_q">(1 0 0 0 for unknown)</field>
+    <field type="float[3]" name="rates">(0 0 0 for unknown)</field>
+    <field type="float[3]" name="position_cov">eph epv</field>
+    <field type="uint64_t" name="custom_state">button states or switches of a tracker device</field>
+]]
+end
+
+-- display welcome message
+gcs:send_text(MAV_SEVERITY.INFO, "follow-target-send script loaded")
+
+-- update function to receive location from payload and move vehicle to reduce payload's oscillation
+local function update()
+
+    -- exit immediately if not enabled
+    if (FOLT_ENABLE:get() <= 0) then
+        return update, 1000
+    end
+
+    -- send FOLLOW_TARGET message
+    send_follow_target_msg()
+
+    return update, UPDATE_INTERVAL_MS
+end
+
+return update()

libraries/AP_Scripting/applets/follow-target-send.md

@@ -0,0 +1,15 @@
+# Follow Target Send
+
+Sends the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one
+
+# Parameters
+
+- FOLT_ENABLE : Set to 1 to enable this script
+- FOLT_MAV_CHAN : MAVLink channel to which FOLLOW_TARGET should be sent
+
+# How To Use
+
+1. copy this script to the autopilot's "scripts" directory
+2. within the "scripts" directory create a "modules" directory
+3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory
+4. the FOLLOW_TARGET message will be published at 10hz