AP_Scripting: added bindings for frsky telemetry library

libraries/AP_Scripting/examples/frsky_rpm.lua

@@ -0,0 +1,56 @@
+--[[
+  Simple example that sends rpm info down the frsky link
+  it works with SPort using SERIAL_PROTOCOL=4,10 and with
+  FPort using SERIAL_PROTOCOL=23.
+  
+  We'll be using OpenTX genuine RPM data IDs (https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h)
+  
+  RPM_FIRST_ID  0x0500
+  RPM_LAST_ID   0x050F
+  
+  and we'll be responding to unused sensor IDs.
+  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 4 (0xE4), 
+  Note: 4 is the index, 0xE4 is the actual ID
+--]]
+
+local loop_time = 250 -- number of ms between runs
+local sport_data_frame = 0x10
+local sensor_id = 0xE4
+--[[
+  we use the last 2 data ids so we hopefully don't interfere with pre existing sensors
+  this is a suggestion but if we know our RPM sensors are going to be
+  the only RPM sensors on the bus we can use 0x0500 and 0x0501
+--]]
+local data_ids = {
+  [0] = 0x050E,
+  [1] = 0x050F,
+}
+
+local function rpm_data_sent(instance, rpm)
+  gcs:send_text(7, string.format("rpm_data_sent() %d: %s", instance, tostring(rpm)))
+end
+
+local function send_rpm_data(instance, callback)
+  local rpm = RPM:get_rpm(instance)
+  gcs:send_text(7,string.format("send_rpm_data() %d, %s", instance, tostring(rpm)))
+  if rpm ~= nil then
+    if frsky_sport:sport_telemetry_push(sensor_id, sport_data_frame, data_ids[instance], rpm) then
+      callback(instance, rpm)
+    end
+  end
+end
+
+local rpm_idx = 0
+
+function update()
+    send_rpm_data(rpm_idx, rpm_data_sent)
+    rpm_idx = (rpm_idx+1)%2
+    return update, loop_time
+end
+
+return update() -- run immediately before starting to reschedule

libraries/AP_Scripting/examples/frsky_wp.lua

@@ -0,0 +1,113 @@
+--[[
+  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

libraries/AP_Scripting/generator/description/bindings.desc

@@ -261,7 +261,7 @@ singleton AP_Mission scheduler-semaphore
 singleton AP_Mission enum MISSION_STOPPED MISSION_RUNNING MISSION_COMPLETE
 singleton AP_Mission method state uint8_t
 singleton AP_Mission method get_current_nav_index uint16_t
-singleton AP_Mission method set_current_cmd boolean uint16_t 0 (ud->num_commands()-1) 
+singleton AP_Mission method set_current_cmd boolean uint16_t 0 (ud->num_commands()-1)
 singleton AP_Mission method get_prev_nav_cmd_id uint16_t
 singleton AP_Mission method get_current_nav_id uint16_t
 singleton AP_Mission method get_current_do_cmd_id uint16_t
@@ -294,7 +294,6 @@ include AP_Button/AP_Button.h
 singleton AP_Button alias button
 singleton AP_Button method get_button_state boolean uint8_t 1 AP_BUTTON_NUM_PINS
 
-
 include AP_Notify/ScriptingLED.h
 singleton ScriptingLED alias LED
 singleton ScriptingLED method get_rgb void uint8_t'Ref uint8_t'Ref uint8_t'Ref
@@ -305,7 +304,12 @@ singleton QuadPlane depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)
 singleton QuadPlane method in_vtol_mode boolean
 
 include AP_Motors/AP_MotorsMatrix.h
-singleton AP_MotorsMatrix depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_TYPE(APM_BUILD_ArduCopter) 
+singleton AP_MotorsMatrix depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_TYPE(APM_BUILD_ArduCopter)
 singleton AP_MotorsMatrix alias MotorsMatrix
 singleton AP_MotorsMatrix method init boolean uint8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1)
 singleton AP_MotorsMatrix method add_motor_raw void int8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1) float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX uint8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1)
+
+include AP_Frsky_Telem/AP_Frsky_SPort.h
+singleton AP_Frsky_SPort alias frsky_sport
+singleton AP_Frsky_SPort method sport_telemetry_push boolean uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX uint16_t 0 UINT16_MAX int32_t -INT32_MAX INT32_MAX
+singleton AP_Frsky_SPort method prep_number uint16_t int32_t INT32_MIN INT32_MAX uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX