From a2e3ade29e6fd72e3d2696c884e5e24647f107c2 Mon Sep 17 00:00:00 2001
From: George Zogopoulos <tailwhipx4@gmail.com>
Date: Thu, 14 Nov 2024 18:02:29 +0100
Subject: [PATCH] AP_Scripting: Added VSPeak Modell flow meter driver

---
 .../drivers/VSPeak_flow_meter.lua             | 256 ++++++++++++++++++
 .../AP_Scripting/drivers/VSPeak_flow_meter.md |  38 +++
 2 files changed, 294 insertions(+)
 create mode 100644 libraries/AP_Scripting/drivers/VSPeak_flow_meter.lua
 create mode 100644 libraries/AP_Scripting/drivers/VSPeak_flow_meter.md

diff --git a/libraries/AP_Scripting/drivers/VSPeak_flow_meter.lua b/libraries/AP_Scripting/drivers/VSPeak_flow_meter.lua
new file mode 100644
index 00000000000000..65bd5c69adfd5f
--- /dev/null
+++ b/libraries/AP_Scripting/drivers/VSPeak_flow_meter.lua
@@ -0,0 +1,256 @@
+-- VSPeak_flow_meter.lua: Driver for the VSPeak Modell fuel flow sensor.
+--
+-- Setup
+--   1. Place this script in the "scripts" directory of the autopilot.
+--   2. Connect the sensor to a serial port (for now referred to as SERIAL*)
+--   3. Enable the scripting engine via SCR_ENABLE.
+--   4. Set SERIAL*_BAUD = 19 (19200)
+--   5. Set SERIAL*_PROTOCOL = 28 (Scripting)
+--   6. Set EFI_TYPE=7 (Scripting)
+--   7. Set BATT2_MONITOR = 27 (EFI)
+
+--
+-- Usage
+--   No further action required.
+
+--[[
+Global definitions
+--]]
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+local SCRIPT_NAME = "VSPeak Modell flow meter driver"
+local NUM_SCRIPT_PARAMS = 1
+local LOOP_RATE_HZ = 10
+local last_warning_time_ms = uint32_t(0)
+local WARNING_DEADTIME_MS = 1000
+local FRAME_LEN = 7
+local BUFFER_LEN = 2*FRAME_LEN - 1
+local HEADER_BYTE = 0xFE
+local INIT_DELAY_MS = 5000
+local uart
+local efi_backend
+local state = {}
+
+
+-- State machine states.
+local FSM_STATE = {
+    INACTIVE = 0,
+    ACTIVE = 1,
+}
+local current_state = FSM_STATE.INACTIVE
+local next_state = FSM_STATE.INACTIVE
+
+--[[
+New parameter declarations
+--]]
+local PARAM_TABLE_KEY = 142 -- Ensure no other applet is using this key.
+local PARAM_TABLE_PREFIX = "VSPF_"
+
+-- Bind a parameter to a variable.
+function bind_param(name)
+   return Parameter(name)
+end
+
+-- Add a parameter and bind it to a variable.
+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
+
+-- Add param table.
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, NUM_SCRIPT_PARAMS), SCRIPT_NAME .. ': Could not add param table.')
+
+--[[
+  // @Param: ENABLE
+  // @DisplayName: Enable this script
+  // @Description: When set to 0 this script will not run. When set to 1 this script will run.
+  // @Range: 0 1
+  // @User: Standard
+--]]
+local VSPF_ENABLE = bind_add_param('ENABLE', 1, 0)
+
+--[[
+Potential additions:
+- The scripting switch.
+--]]
+-- Warn the user, throttling the message rate.
+function warn_user(msg, severity)
+    severity = severity or MAV_SEVERITY.WARNING -- Optional severity argument.
+    if millis() - last_warning_time_ms > WARNING_DEADTIME_MS then
+            gcs:send_text(severity, SCRIPT_NAME .. ": " .. msg)
+        last_warning_time_ms = millis()
+    end
+end
+
+-- Get uint16 from two bytes
+function uint16_value(hbyte, lbyte)
+  return ((hbyte & 0xFF) << 8) | (lbyte & 0xFF)
+end
+
+function read_uart()
+    local n_avail = uart:available()
+
+    -- Discard up to BUFFER_LEN bytes.
+    -- These are stale data.
+    if (n_avail - BUFFER_LEN) > 0 then
+        n_avail = BUFFER_LEN
+        for _ = 1, (n_avail-BUFFER_LEN) do
+            uart:read()
+        end
+    end
+
+    -- Read in the rest of the data.
+    for _ = 1, n_avail do
+        local val = uart:read()
+        table.insert(state.buffer, val)
+    end
+
+end
+
+function parse_buffer()
+
+    -- Drop old data.
+    for _ = 1, (#state.buffer - BUFFER_LEN) do
+        table.remove(state.buffer, 1)
+    end
+
+    -- Find the header byte and discard up to it.
+    for _ = 1, #state.buffer do
+        if state.buffer[1] ~= HEADER_BYTE then
+            table.remove(state.buffer, 1)
+        else
+            break
+        end
+    end
+    
+    -- Check if the buffer has enough data in it.
+    if #state.buffer >= FRAME_LEN then
+        -- Parse the data.
+        state.flow = uint16_value(state.buffer[3], state.buffer[2])
+        state.fuel_ml = uint16_value(state.buffer[5], state.buffer[4])
+        state.fuel_pct = state.buffer[6]
+        state.checksum = state.buffer[7]
+        -- Calculate the checksum.
+        local checksum = 0x00
+        for i = 1, 6 do
+            checksum = (checksum ~ state.buffer[i]) & 0xFF
+        end
+
+        -- If parse successful, discard them.
+        if checksum == state.checksum then
+            state.updated = true
+            for _ = 1, FRAME_LEN do
+                table.remove(state.buffer, 1)
+            end
+        else
+            warn_user("Checksum failed")
+            state.updated = false
+            -- Discard the header to force the corrupt data to flush on the next pass.
+            table.remove(state.buffer, 1)
+        end
+        
+    end
+
+end
+
+function update_efi()
+   if state.updated == false then
+      return
+   end
+
+   local efi_state = EFI_State()
+   efi_state:fuel_consumption_rate_cm3pm(state.flow)
+   efi_state:estimated_consumed_fuel_volume_cm3(state.fuel_ml)
+   efi_state:last_updated_ms(millis())
+   efi_backend:handle_scripting(efi_state)
+end
+
+--[[
+Activation conditions
+--]]
+-- Check for script activating conditions here.
+function can_start()
+    return VSPF_ENABLE:get() == 1 and (millis() > INIT_DELAY_MS)
+end
+
+--[[
+Deactivation conditions
+--]]
+-- Check for script deactivating conditions here.
+function must_stop()
+    return VSPF_ENABLE:get() == 0
+end
+
+--[[
+State machine
+--]]
+-- Write the state machine transitions.
+function fsm_step()
+    if current_state == FSM_STATE.INACTIVE then
+
+        if can_start() then
+            setup()
+            next_state = FSM_STATE.ACTIVE
+        end
+
+    elseif current_state == FSM_STATE.ACTIVE then
+
+        -- Feed the buffer.
+        read_uart()
+        parse_buffer()
+        update_efi()
+
+        if must_stop() then
+            next_state = FSM_STATE.INACTIVE
+        end
+	end
+    current_state = next_state
+end
+
+--[[
+Main loop function
+--]]
+function update()
+	-- Stuff is placed here.
+    fsm_step()
+end
+
+--[[
+Setup function
+--]]
+function setup()
+    uart = serial:find_serial(0) -- First scripting serial
+    if not uart then
+        warn_user("Unable to find scripting serial", MAV_SEVERITY.ERROR)
+        return
+    end
+    uart:begin(19200)
+
+    efi_backend = efi:get_backend(0)
+    if not efi_backend then
+        warn_user("Unable to find EFI backend", MAV_SEVERITY.ERROR)
+        return
+    end
+
+    state.last_read_us = 0x00
+    state.buffer = {}
+    state.flow = 0x00
+    state.fuel_ml = 0x00
+    state.fuel_pct = 0x00
+    state.checksum = 0x00
+    state.updated = false
+end
+
+gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME .. string.format(" loaded."))
+
+-- Wrapper around update() to catch errors.
+function protected_wrapper()
+  local success, err = pcall(update)
+  if not success then
+    gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err)
+    return protected_wrapper, 1000
+  end
+  return protected_wrapper, 1000.0/LOOP_RATE_HZ
+end
+
+-- Start running update loop
+return protected_wrapper()
diff --git a/libraries/AP_Scripting/drivers/VSPeak_flow_meter.md b/libraries/AP_Scripting/drivers/VSPeak_flow_meter.md
new file mode 100644
index 00000000000000..b5febbace405d5
--- /dev/null
+++ b/libraries/AP_Scripting/drivers/VSPeak_flow_meter.md
@@ -0,0 +1,38 @@
+# VSPeak Modell flow meter Driver
+
+This driver implements support for the VSPeak Modell flow meter sensor.
+
+https://www.vspeak-modell.de/en/flow-meter
+
+# Parameters
+
+The script used the following parameters:
+
+## VSPF_ENABLE
+
+Setting this to 1 enables the driver.
+
+# Setup
+
+First of all, calibrate and configure the flow meter according to the
+manufacturer instructions. Set your configuration with the `FLOW.txt` file,
+placed in the SD card in the sensor itself.
+
+Once this is done, perform the following steps.
+
+1. Place this script in the "scripts" directory of the autopilot.
+2. Connect the sensor to a serial port (for now referred to as `SERIAL*`)
+3. Enable the scripting engine via `SCR_ENABLE`.
+4. Set the baud rate to 19200 with `SERIAL*_BAUD = 19`.
+5. Set port protocol to scripting with `SERIAL*_PROTOCOL = 28`.
+6. Set the EFI type to scripting with `EFI_TYPE = 7`.
+7. Set a battery monitor to EFI. For example, to set the 2nd battery monitor
+    use `BATT2_MONITOR = 27`.
+8. Enable the script itself with `VSPF_ENABLE=1`.
+
+# Operation
+
+Once everything is configured correctly, the corresponding battery monitor
+will display in the corresponding `BATTERY_STATUS` MAVLink message:
+ - The current fuel flow in cl/hr (centiliters per hour) in the `current_battery` field.
+ - The current fuel already consumed in ml (milliliters) in the `current_consumed` field.