ardupilot/libraries/AP_Scripting/drivers/Generator_SVFFI.lua

--[[
   SVFFI serial protocol for generator support
   See http://www.svffi.com/en/
--]]

local PARAM_TABLE_KEY = 42
local PARAM_TABLE_PREFIX = "EFI_SVF_"

local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}

-- bind a parameter to a variable given
local function bind_param(name)
    local p = Parameter()
    assert(p:init(name), string.format('could not find %s parameter', name))
    return p
end

-- 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', name))
    return bind_param(PARAM_TABLE_PREFIX .. name)
end

-- setup script specific parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 8), 'could not add param table')

--[[
  // @Param: EFI_SVF_ENABLE
  // @DisplayName: Generator SVFFI enable
  // @Description: Enable SVFFI generator support
  // @Values: 0:Disabled,1:Enabled
  // @User: Standard
--]]
EFI_SVF_ENABLE = bind_add_param("ENABLE", 1, 0)

--[[
  // @Param: EFI_SVF_ARMCHECK
  // @DisplayName: Generator SVFFI arming check
  // @Description: Check for Generator ARM state before arming
  // @Values: 0:Disabled,1:Enabled
  // @User: Standard
--]]
EFI_SVF_ARMCHECK = bind_add_param("ARMCHECK", 2, 1)

if EFI_SVF_ENABLE:get() ~= 1 then
   return
end

local auth_id = arming:get_aux_auth_id()
arming:set_aux_auth_failed(auth_id, "GEN: not in ARM state")


local uart = serial:find_serial(0) -- first scripting serial
if not uart then
   gcs:send_text(MAV_SEVERITY.ERROR, "GEN_SVF: unable to find serial port")
   return
end
uart:begin(115200)

local efi_backend = efi:get_backend(0)
if not efi_backend then
   gcs:send_text(MAV_SEVERITY.ERROR, "GEN_SVF: unable to find EFI backend")
   return
end

local function read_bytes(n)
   local ret = ""
   for _ = 1, n do
      ret = ret .. string.char(uart:read())
   end
   return ret
end

local auchCRCHi = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40
}

local auchCRCLo = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
    0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
    0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
    0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
    0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
    0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
    0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
    0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
    0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
    0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
    0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
    0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
    0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
    0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
    0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
    0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
    0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
    0x41, 0x81, 0x80, 0x40
}

--[[
   calculate crc16
--]]
local function get_crc16(s)
   local uchCRCHi = 0xFF
   local uchCRCLo = 0xFF
   for i = 1, #s do
      local b = string.byte(string.sub(s, i, i))
      local uIndex = uchCRCLo ~ b
      --gcs:send_text(MAV_SEVERITY.INFO, string.format("uIndex=%u", uIndex))
      uchCRCLo = uchCRCHi ~ auchCRCHi[uIndex+1]
      uchCRCHi = auchCRCLo[uIndex+1]
   end
   return (uchCRCHi << 8 | uchCRCLo)
end

local state = {}
state.last_read_us = uint32_t(0)
state.last_status = -1


--[[
   check for input and parse data
--]]
local function check_input()
   local n_bytes = uart:available():toint()
   --gcs:send_text(MAV_SEVERITY.INFO, string.format("n_bytes=%u %.2f", n_bytes, millis():tofloat()*0.001))
   if n_bytes < 31 then
      return
   end

   local s = read_bytes(n_bytes)
   local prefix, len = string.unpack("<HB", s, 1)
   if prefix ~= 0xa55a then
      gcs:send_text(MAV_SEVERITY.INFO, string.format("bad prefix 0x%x", prefix))
      return
   end
   if len+5 ~= n_bytes then
      gcs:send_text(MAV_SEVERITY.INFO, string.format("bad len %u %u", n_bytes, len))
      return
   end
   local crc = string.unpack("<H", s, 4+len)
   local s2 = string.sub(s,1,n_bytes-2)
   --gcs:send_text(MAV_SEVERITY.INFO, string.format("s2 n_bytes=%u len1=%u len2=%u", n_bytes, #s, #s2))
   local crc2 = get_crc16(s2)
   if crc ~= crc2 then
      gcs:send_text(MAV_SEVERITY.INFO, string.format("bad crc %x %x", crc, crc2))
      return
   end

   state.version, state.rpm, state.throttle = string.unpack("<BHH", string.sub(s,4,8))
   state.voltage, state.current, state.runtime = string.unpack("<HHI", string.sub(s,9,16))
   state.maint_time, state.lock_time, state.status = string.unpack("<HHB", string.sub(s,17,21))
   state.alarm, state.fuellevel, state.cht1 = string.unpack("<HBH", string.sub(s,22,26))
   state.cht2, state.pcb_temp = string.unpack("<HB", string.sub(s,27,29))

   if state.status ~= state.last_status then
      state.last_status = state.status
      local states = {"STOP", "IDLE", "RUN", "3", "CHARGE" }
      local name = states[state.status+1]
      if not name then
         name = string.format("Unknown%u", state.status)
      end
      gcs:send_text(MAV_SEVERITY.WARNING, string.format("Generator state: %s", name))
      if name ~= "RUN" and EFI_SVF_ARMCHECK:get() == 1 then
         arming:set_aux_auth_failed(auth_id, string.format("GEN: not in ARM state (%s)", name))
      else
         arming:set_aux_auth_passed(auth_id)
      end

   end

   state.last_read_us = micros()
end

--[[
   update EFI state
--]]
local function update_EFI()
   if state.last_read_us == uint32_t(0) then
      return
   end
   local cylinder_state = Cylinder_Status()
   local efi_state = EFI_State()
   local C_TO_KELVIN = 273.2

   cylinder_state:cylinder_head_temperature(state.cht1+C_TO_KELVIN)
   efi_state:engine_speed_rpm(state.rpm)

   efi_state:throttle_position_percent(state.throttle)
   efi_state:ignition_voltage(state.voltage*0.1)

   efi_state:cylinder_status(cylinder_state)
   efi_state:last_updated_ms(millis())

   -- Set the EFI_State into the EFI scripting driver
   efi_backend:handle_scripting(efi_state)

   gcs:send_named_float('GEN_VOLT', state.voltage*0.1)
   gcs:send_named_float('GEN_AMPS', state.current*0.1)
   gcs:send_named_float('GEN_STAT', state.status)
   logger.write('SVF','Curr,Volt,Status', 'ffB',
                state.current*0.1,
                state.voltage*0.1,
                state.status)


end


--[[
   main update function
--]]
local function update()
   check_input()
   update_EFI()

   return update, 100
end

gcs:send_text(MAV_SEVERITY.INFO, "GEN_SVF: loaded")

return update()