ardupilot/libraries/AP_Scripting/examples/frsky_wp.lua

115 lines
3.4 KiB
Lua

--[[
This example adds a new passthrough packet type for waypoints.
Waypoint data is packet into 32bits and sent down the frsky bus with a DIY appid of 0x5009.
- 10 bits for current waypoint index, max 1023
- 12 bits for the distance in meters encoded with AP_Frsky_SPort::prep_number(distance, 3, 2) max is 102.3Km
- 6 bits for xtrack error (5 bits + sign) encoded with prep_5bits
- 3 bits for bearing from COG with a 45° resolution
We'll be responding to an unused sensor ID
This is a list of IDs we can't use:
- serial protocol 4 uses IDs 0,2,3 and 6
- serial protocol 10 uses ID 7,13,20,27
- serial protocol 23, no IDs used
For this test we'll use sensor ID 17 (0x71),
Note: 17 is the index, 0x71 is the actual ID
--]]
-- luacheck: only 0
local loop_time = 1000 -- number of ms between runs
local WP_OFFSET_DISTANCE = 10
local WP_OFFSET_BEARING = 29
local WP_OFFSET_XTRACK = 22
local WP_LIMIT_COUNT = 0x3FF -- 1023
local WP_LIMIT_XTRACK = 0x7F -- 127m
local WP_LIMIT_DISTANCE = 0x18F9C -- 102.3Km
local WP_ARROW_COUNT = 8 -- 8 possible directions
local wp_bearing = 0
local wp_index = 0
local wp_distance = 0
local wp_xtrack = 0
function wrap_360(angle)
local res = angle % 360
if res < 0 then
res = res + 360
end
return res
end
function prep_5bits(num)
local res = 0
local abs_num = math.floor(math.abs(num) + 0.5)
if abs_num < 10 then
res = abs_num << 1
elseif abs_num < 150 then
res = ( math.floor((abs_num * 0.1)+0.5) << 1) | 0x1
else
res = 0x1F
end
if num < 0 then
res = res | 0x1 << 5
end
return res
end
function wp_pack(index, distance, bearing, xtrack)
local wp_dword = uint32_t()
wp_dword = math.min(index,WP_LIMIT_COUNT)
wp_dword = wp_dword | frsky_sport:prep_number(math.min(math.floor(distance+0.5),WP_LIMIT_DISTANCE),3,2) << WP_OFFSET_DISTANCE
wp_dword = wp_dword | prep_5bits(math.min(xtrack,WP_LIMIT_XTRACK)) << WP_OFFSET_XTRACK
if gps:status(0) >= gps.GPS_OK_FIX_2D then
local cog = gps:ground_course(0) -- deg
local angle = wrap_360(bearing - cog) -- deg
local interval = 360 / WP_ARROW_COUNT -- 45 deg
-- hint from OSD code to avoid unreliable bearing at small distances
if distance < 2 then
angle = 0
end
-- bearing expressed as offset from cog as multiple of 45° ( 8 sectors) encoded as 3bits
wp_dword = wp_dword | ((math.floor(((angle + interval/2) / interval)) % WP_ARROW_COUNT) & 0x7) << WP_OFFSET_BEARING
end
return wp_dword & 0xFFFFFFFF
end
function update_wp_info()
local index = mission:get_current_nav_index()
local distance = vehicle:get_wp_distance_m()
local bearing = vehicle:get_wp_bearing_deg()
local xtrack = vehicle:get_wp_crosstrack_error_m()
if index ~= nil and distance ~= nil and bearing ~= nil and xtrack ~= nil then
wp_index = index
wp_bearing = bearing
wp_distance = distance
wp_xtrack = xtrack
return true
end
return false
end
function update()
local gps_status = gps.status(0,0)
if not update_wp_info() then
return update, loop_time
end
local sensor_id = 0x71
local wp_dword = uint32_t()
wp_dword = wp_pack(wp_index, wp_distance, wp_bearing, wp_xtrack)
frsky_sport:sport_telemetry_push(sensor_id, 0x10, 0x5009, wp_dword)
return update, loop_time
end
return update() , 1000