You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
113 lines
3.3 KiB
113 lines
3.3 KiB
--[[ |
|
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 |
|
--]] |
|
|
|
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
|
|
|