AP_Scripting: fix remaining luacheck issues

This commit is contained in:
Iampete1 2024-12-24 01:34:03 +00:00 committed by Thomas Watson
parent 95d4dc1a0d
commit 0dbbb87ce2
26 changed files with 57 additions and 144 deletions

View File

@ -8,7 +8,6 @@
---@diagnostic disable: param-type-mismatch ---@diagnostic disable: param-type-mismatch
---@diagnostic disable: undefined-field ---@diagnostic disable: undefined-field
---@diagnostic disable: missing-parameter ---@diagnostic disable: missing-parameter
---@diagnostic disable: cast-local-type
---@diagnostic disable: need-check-nil ---@diagnostic disable: need-check-nil
---@diagnostic disable: undefined-global ---@diagnostic disable: undefined-global
---@diagnostic disable: inject-field ---@diagnostic disable: inject-field
@ -659,8 +658,8 @@ end
--[[ --[[
create a class that inherits from a base class create a class that inherits from a base class
--]] --]]
local function inheritsFrom(baseClass, _name) local function inheritsFrom(baseClass, name_in)
local new_class = { name = _name } local new_class = { name = name_in }
local class_mt = { __index = new_class } local class_mt = { __index = new_class }
function new_class:create() function new_class:create()
@ -1658,10 +1657,8 @@ end
--[[ --[[
perform a rudder over maneuver perform a rudder over maneuver
--]] --]]
function rudder_over(_direction, _min_speed) function rudder_over(direction, min_speed)
local self = {} local self = {}
local direction = _direction
local min_speed = _min_speed
local reached_speed = false local reached_speed = false
local kick_started = false local kick_started = false
local pitch2_done = false local pitch2_done = false
@ -1822,12 +1819,10 @@ end
--[[ --[[
takeoff controller takeoff controller
--]] --]]
function takeoff_controller(_distance, _thr_slew) function takeoff_controller(distance, thr_slew)
local self = {} local self = {}
local start_time = 0 local start_time = 0
local start_pos = nil local start_pos = nil
local thr_slew = _thr_slew
local distance = _distance
local all_done = false local all_done = false
local initial_yaw_deg = math.deg(ahrs:get_yaw()) local initial_yaw_deg = math.deg(ahrs:get_yaw())
local yaw_correction_tconst = 1.0 local yaw_correction_tconst = 1.0
@ -2245,11 +2240,9 @@ end
milliseconds means we lose accuracy over time. At 9 hours we have milliseconds means we lose accuracy over time. At 9 hours we have
an accuracy of about 1 millisecond an accuracy of about 1 millisecond
--]] --]]
local function JitterCorrection(_max_lag_ms, _convergence_loops) local function JitterCorrection(max_lag_ms, convergence_loops)
local self = {} local self = {}
local max_lag_ms = _max_lag_ms
local convergence_loops = _convergence_loops
local link_offset_ms = 0 local link_offset_ms = 0
local min_sample_ms = 0 local min_sample_ms = 0
local initialised = false local initialised = false

View File

@ -4,8 +4,6 @@
-- this pot to adjust the sensitivity of the collective about the collective midstick. The 2nd Pot then controls -- this pot to adjust the sensitivity of the collective about the collective midstick. The 2nd Pot then controls
-- the value of the 50% point on the curve. This can be used to set the collective position to aid with hovering -- the value of the 50% point on the curve. This can be used to set the collective position to aid with hovering
-- at the midstick. -- at the midstick.
-- luacheck: only 0
---@diagnostic disable: cast-local-type
-- Tested and working as of 25th Aug 2020 (Copter Dev) -- Tested and working as of 25th Aug 2020 (Copter Dev)
@ -83,8 +81,7 @@ end
-- Get parameter value and perform checks to ensure successful -- Get parameter value and perform checks to ensure successful
function get_im_val(param_name,disp) function get_im_val(param_name,disp)
local value = -1 local value = param:get(param_name)
value = param:get(param_name)
if value >= 0 then if value >= 0 then
if disp then if disp then

View File

@ -4,8 +4,6 @@
-- Throttle thresholds also allow automatic enable and disable of stop motors -- Throttle thresholds also allow automatic enable and disable of stop motors
-- slew up and down times allow to configure how fast the motors are disabled and re-enabled -- slew up and down times allow to configure how fast the motors are disabled and re-enabled
-- luacheck: only 0
---@diagnostic disable: cast-local-type
-- Config -- Config
@ -39,12 +37,6 @@ if throttle_off_threshold < 100 then
assert((throttle_off_threshold < throttle_on_threshold) and (throttle_on_threshold < 100), "throttle on and off thresholds not configured correctly") assert((throttle_off_threshold < throttle_on_threshold) and (throttle_on_threshold < 100), "throttle on and off thresholds not configured correctly")
end end
-- clear vars we don't need anymore
slew_down_time = nil
slew_up_time = nil
pwm_range = nil
for i = 1, #stop_motors do for i = 1, #stop_motors do
-- Check for a valid motor number -- Check for a valid motor number
assert(stop_motors[i] >= 1 and stop_motors[i] <= 12, string.format("Lua: motor %i not valid",stop_motors[i])) assert(stop_motors[i] >= 1 and stop_motors[i] <= 12, string.format("Lua: motor %i not valid",stop_motors[i]))
@ -84,7 +76,7 @@ assert(#stop_motor_chan == #stop_motors, "Lua: could not find all motors to stop
assert(#run_motor_fun > 0, "Lua: cannot stop all motors") assert(#run_motor_fun > 0, "Lua: cannot stop all motors")
-- keep track of last time in a VTOL mode, this allows to delay switching after a transition/assist -- keep track of last time in a VTOL mode, this allows to delay switching after a transition/assist
local last_vtol_active = 0 local last_vtol_active = uint32_t(0)
-- current action -- current action
local script_enabled = false local script_enabled = false

View File

@ -435,13 +435,11 @@ end
--[[ --[[
client class for open connections client class for open connections
--]] --]]
local function Client(_sock, _idx) local function Client(sock, idx)
local self = {} local self = {}
self.closed = false self.closed = false
local sock = _sock
local idx = _idx
local have_header = false local have_header = false
local header = "" local header = ""
local header_lines = {} local header_lines = {}

View File

@ -1,16 +1,12 @@
--[[ --[[
support package place for quadplanes support package place for quadplanes
--]] --]]
-- luacheck: only 0
---@diagnostic disable: param-type-mismatch
local PARAM_TABLE_KEY = 9 local PARAM_TABLE_KEY = 9
local PARAM_TABLE_PREFIX = "PKG_" local PARAM_TABLE_PREFIX = "PKG_"
local MODE_AUTO = 10 local MODE_AUTO = 10
local NAV_TAKEOFF = 22
local NAV_VTOL_PAYLOAD_PLACE = 94 local NAV_VTOL_PAYLOAD_PLACE = 94
-- add a parameter and bind it to a variable -- add a parameter and bind it to a variable
@ -31,7 +27,6 @@ local Q_LAND_FINAL_ALT = Parameter("Q_LAND_FINAL_ALT")
local MAV_SEVERITY_INFO = 6 local MAV_SEVERITY_INFO = 6
local MAV_SEVERITY_NOTICE = 5 local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_EMERGENCY = 0
local RNG_ORIENT_DOWN = 25 local RNG_ORIENT_DOWN = 25

View File

@ -274,7 +274,7 @@ function protected_wrapper()
gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err) gcs:send_text(MAV_SEVERITY.EMERGENCY, "Internal Error: " .. err)
return protected_wrapper, 1000 return protected_wrapper, 1000
end end
if not (current_state == FSM_STATE.FINISHED) then if current_state ~= FSM_STATE.FINISHED then
return protected_wrapper, 1000.0/LOOP_RATE_HZ return protected_wrapper, 1000.0/LOOP_RATE_HZ
end end
end end

View File

@ -1,9 +1,7 @@
--[[ --[[
EFI Scripting backend driver for HFE based on HFEDCN0191 Rev E EFI Scripting backend driver for HFE based on HFEDCN0191 Rev E
--]] --]]
-- luacheck: only 0
---@diagnostic disable: param-type-mismatch ---@diagnostic disable: param-type-mismatch
---@diagnostic disable: redundant-parameter
---@diagnostic disable: undefined-field ---@diagnostic disable: undefined-field
---@diagnostic disable: missing-parameter ---@diagnostic disable: missing-parameter
---@diagnostic disable: need-check-nil ---@diagnostic disable: need-check-nil
@ -98,7 +96,7 @@ local now_s = get_time_sec()
--[[ --[[
EFI Engine Object EFI Engine Object
--]] --]]
local function engine_control(_driver) local function engine_control(driver)
local self = {} local self = {}
-- Build up the EFI_State that is passed into the EFI Scripting backend -- Build up the EFI_State that is passed into the EFI Scripting backend
@ -109,7 +107,6 @@ local function engine_control(_driver)
local rpm = 0 local rpm = 0
local air_pressure = 0 local air_pressure = 0
local map_ratio = 0.0 local map_ratio = 0.0
local driver = _driver
local last_rpm_t = get_time_sec() local last_rpm_t = get_time_sec()
local last_state_update_t = get_time_sec() local last_state_update_t = get_time_sec()
local throttle_pos = 0.0 local throttle_pos = 0.0
@ -123,12 +120,6 @@ local function engine_control(_driver)
local injector_duty = 0.0 local injector_duty = 0.0
local ignition_angle = 0.0 local ignition_angle = 0.0
-- Generator Data Structure
local gen = {}
gen.amps = 0.0
gen.rpm = 0.0
gen.batt_current = 0.0
-- Temperature Data Structure -- Temperature Data Structure
local temps = {} local temps = {}
temps.egt = 0.0 -- Engine Gas Temperature temps.egt = 0.0 -- Engine Gas Temperature
@ -275,9 +266,9 @@ local function engine_control(_driver)
-- return the instance -- return the instance
return self return self
end -- end function engine_control(_driver) end -- end function engine_control(driver)
local engine1 = engine_control(driver1, 1) local engine1 = engine_control(driver1)
function update() function update()
now_s = get_time_sec() now_s = get_time_sec()

View File

@ -338,7 +338,7 @@ local function engine_control()
-- return the instance -- return the instance
return self return self
end -- end function engine_control(_driver) end -- end function engine_control()
local engine1 = engine_control() local engine1 = engine_control()

View File

@ -243,7 +243,7 @@ end
--[[ --[[
EFI Engine Object EFI Engine Object
--]] --]]
local function engine_control(_driver) local function engine_control(driver)
local self = {} local self = {}
-- Build up the EFI_State that is passed into the EFI Scripting backend -- Build up the EFI_State that is passed into the EFI Scripting backend
@ -263,7 +263,6 @@ local function engine_control(_driver)
local fuel_consumption_lph = 0 local fuel_consumption_lph = 0
local fuel_total_l = 0 local fuel_total_l = 0
local last_fuel_s = 0 local last_fuel_s = 0
local driver = _driver
local last_rpm_t = get_time_sec() local last_rpm_t = get_time_sec()
local last_state_update_t = get_time_sec() local last_state_update_t = get_time_sec()
local last_thr_update = get_time_sec() local last_thr_update = get_time_sec()

View File

@ -2,7 +2,6 @@
Script to control LED strips based on the roll of the aircraft. This is an example to demonstrate Script to control LED strips based on the roll of the aircraft. This is an example to demonstrate
the LED interface for WS2812 LEDs the LED interface for WS2812 LEDs
--]] --]]
-- luacheck: only 0
--[[ --[[
for this demo we will use a single strip with 30 LEDs for this demo we will use a single strip with 30 LEDs
@ -77,7 +76,7 @@ id[7][6] = 28
id[7][7] = 27 id[7][7] = 27
-- ArduPilot logo 7 x 48, RGB -- ArduPilot logo 7 x 48, RGB
image = {} local image = {}
image[1] = {} image[1] = {}
image[2] = {} image[2] = {}
image[3] = {} image[3] = {}
@ -502,9 +501,6 @@ local function display_image(image_in,offset_in,brightness_in)
brightness = brightness_in brightness = brightness_in
end end
local i
local j
for i = 1, 48 do for i = 1, 48 do
local x_index = i + im_offset local x_index = i + im_offset
if x_index >= 1 and x_index <= matrix_x then if x_index >= 1 and x_index <= matrix_x then

View File

@ -2,7 +2,6 @@
Script to control LED strips based on the roll of the aircraft. This is an example to demonstrate Script to control LED strips based on the roll of the aircraft. This is an example to demonstrate
the LED interface for WS2812 LEDs the LED interface for WS2812 LEDs
--]] --]]
-- luacheck: only 0
--[[ --[[
for this demo we will use a single strip with 30 LEDs for this demo we will use a single strip with 30 LEDs
@ -77,7 +76,7 @@ id[7][6] = 28
id[7][7] = 27 id[7][7] = 27
-- https://github.com/noopkat/oled-font-5x7/blob/master/oled-font-5x7.js -- https://github.com/noopkat/oled-font-5x7/blob/master/oled-font-5x7.js
font = {} local font = {}
font[' '] = {0x00, 0x00, 0x00, 0x00, 0x00} -- // space font[' '] = {0x00, 0x00, 0x00, 0x00, 0x00} -- // space
font['!'] = {0x00, 0x00, 0x5F, 0x00, 0x00} -- // ! font['!'] = {0x00, 0x00, 0x5F, 0x00, 0x00} -- // !
font['"'] = {0x00, 0x07, 0x00, 0x07, 0x00} -- // " font['"'] = {0x00, 0x07, 0x00, 0x07, 0x00} -- // "
@ -205,9 +204,6 @@ if char_offset < 1 - 5 then
return return
end end
local i
local j
for i = 1, 5 do for i = 1, 5 do
local x_index = i + char_offset local x_index = i + char_offset
if x_index >= 1 and x_index <= matrix_x then if x_index >= 1 and x_index <= matrix_x then
@ -225,7 +221,6 @@ end
local function display_string(string,r,g,b,offset_in) local function display_string(string,r,g,b,offset_in)
local str_offset = 0 local str_offset = 0
local i
for i = 1, string:len() do for i = 1, string:len() do
display_char(string:sub(i,i),r,g,b,str_offset + offset_in) display_char(string:sub(i,i),r,g,b,str_offset + offset_in)
str_offset = str_offset + 6 str_offset = str_offset + 6

View File

@ -2,7 +2,6 @@
Script to control LED strips based on the roll of the aircraft. This is an example to demonstrate Script to control LED strips based on the roll of the aircraft. This is an example to demonstrate
the LED interface for WS2812 LEDs the LED interface for WS2812 LEDs
--]] --]]
-- luacheck: only 0
--[[ --[[
@ -57,7 +56,7 @@ local rainbow = {
--[[ --[[
Function to set a LED to a color on a classic rainbow spectrum, with v=0 giving red Function to set a LED to a color on a classic rainbow spectrum, with v=0 giving red
--]] --]]
function set_Rainbow(chan, led, v) local function set_Rainbow(channel, led, v)
local num_rows = #rainbow local num_rows = #rainbow
local row = math.floor(constrain(v * (num_rows-1)+1, 1, num_rows-1)) local row = math.floor(constrain(v * (num_rows-1)+1, 1, num_rows-1))
local v0 = (row-1) / (num_rows-1) local v0 = (row-1) / (num_rows-1)
@ -66,7 +65,7 @@ function set_Rainbow(chan, led, v)
r = math.floor(rainbow[row][1] + p * (rainbow[row+1][1] - rainbow[row][1])) r = math.floor(rainbow[row][1] + p * (rainbow[row+1][1] - rainbow[row][1]))
g = math.floor(rainbow[row][2] + p * (rainbow[row+1][2] - rainbow[row][2])) g = math.floor(rainbow[row][2] + p * (rainbow[row+1][2] - rainbow[row][2]))
b = math.floor(rainbow[row][3] + p * (rainbow[row+1][3] - rainbow[row][3])) b = math.floor(rainbow[row][3] + p * (rainbow[row+1][3] - rainbow[row][3]))
serialLED:set_RGB(chan, led, r, g, b) serialLED:set_RGB(channel, led, r, g, b)
end end
--[[ --[[

View File

@ -1,5 +1,4 @@
-- Script decodes, checks and prints NMEA messages -- Script decodes, checks and prints NMEA messages
-- luacheck: only 0
-- find the serial first (0) scripting serial port instance -- find the serial first (0) scripting serial port instance
local port = serial:find_serial(0) local port = serial:find_serial(0)
@ -40,8 +39,8 @@ local function decode_NMEA(byte)
-- test the checksum -- test the checksum
string_complete = true string_complete = true
return checksum == tonumber(term[term_number],16) return checksum == tonumber(term[term_number],16)
else
-- we could further decode the message data here -- else -- we could further decode the message data here
end end
if char == '*' then if char == '*' then

View File

@ -26,13 +26,10 @@
-- SCR_USER4 holds the threshold for optical flow innovations (about 0.15 is a good choice) -- SCR_USER4 holds the threshold for optical flow innovations (about 0.15 is a good choice)
-- --
-- When the 2nd auxiliary switch (300/Scripting1) is pulled high automatic source selection uses these thresholds: -- When the 2nd auxiliary switch (300/Scripting1) is pulled high automatic source selection uses these thresholds:
-- luacheck: only 0
---@diagnostic disable: cast-local-type
---@diagnostic disable: need-check-nil ---@diagnostic disable: need-check-nil
local rangefinder_rotation = 25 -- check downward (25) facing lidar local rangefinder_rotation = 25 -- check downward (25) facing lidar
local source_prev = 0 -- previous source, defaults to primary source local source_prev = 0 -- previous source, defaults to primary source
local sw_source_prev = -1 -- previous source switch position
local sw_auto_pos_prev = -1 -- previous auto source switch position local sw_auto_pos_prev = -1 -- previous auto source switch position
local auto_switch = false -- true when auto switching between sources is active local auto_switch = false -- true when auto switching between sources is active
local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value
@ -115,12 +112,9 @@ function update()
-- get opticalflow innovations from ahrs (only x and y values are valid) -- get opticalflow innovations from ahrs (only x and y values are valid)
local opticalflow_over_threshold = true local opticalflow_over_threshold = true
local opticalflow_xy_innov = 0 local opticalflow_innov = ahrs:get_vel_innovations_and_variances_for_source(5)
local opticalflow_innov = Vector3f()
local opticalflow_var = Vector3f()
opticalflow_innov, opticalflow_var = ahrs:get_vel_innovations_and_variances_for_source(5)
if (opticalflow_innov) then if (opticalflow_innov) then
opticalflow_xy_innov = math.sqrt(opticalflow_innov:x() * opticalflow_innov:x() + opticalflow_innov:y() * opticalflow_innov:y()) local opticalflow_xy_innov = math.sqrt(opticalflow_innov:x() * opticalflow_innov:x() + opticalflow_innov:y() * opticalflow_innov:y())
opticalflow_over_threshold = (opticalflow_xy_innov == 0.0) or (opticalflow_xy_innov > opticalflow_innov_thresh) opticalflow_over_threshold = (opticalflow_xy_innov == 0.0) or (opticalflow_xy_innov > opticalflow_innov_thresh)
end end

View File

@ -9,15 +9,11 @@
-- SCR_USER3 holds the threshold for GPS innovations (around 0.3 is a good choice) -- SCR_USER3 holds the threshold for GPS innovations (around 0.3 is a good choice)
-- if GPS speed accuracy <= SCR_USER2 and GPS innovations <= SRC_USER3 then the GPS (primary source set) will be used -- if GPS speed accuracy <= SCR_USER2 and GPS innovations <= SRC_USER3 then the GPS (primary source set) will be used
-- otherwise wheel encoders (secondary source set) will be used -- otherwise wheel encoders (secondary source set) will be used
-- luacheck: only 0
---@diagnostic disable: cast-local-type
---@diagnostic disable: need-check-nil ---@diagnostic disable: need-check-nil
local source_prev = 0 -- previous source, defaults to primary source local source_prev = 0 -- previous source, defaults to primary source
local sw_source_prev = -1 -- previous source switch position
local sw_auto_pos_prev = -1 -- previous auto source switch position local sw_auto_pos_prev = -1 -- previous auto source switch position
local auto_switch = false -- true when auto switching between sources is active local auto_switch = false -- true when auto switching between sources is active
local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value
local vote_counter_max = 20 -- when a vote counter reaches this number (i.e. 2sec) source may be switched local vote_counter_max = 20 -- when a vote counter reaches this number (i.e. 2sec) source may be switched
local gps_vs_nongps_vote = 0 -- vote counter for GPS vs NonGPS (-20 = GPS, +20 = NonGPS) local gps_vs_nongps_vote = 0 -- vote counter for GPS vs NonGPS (-20 = GPS, +20 = NonGPS)
@ -67,9 +63,7 @@ function update()
local gps_over_threshold = (gps_speed_accuracy == nil) or (gps:speed_accuracy(gps:primary_sensor()) > gps_speedaccuracy_thresh) local gps_over_threshold = (gps_speed_accuracy == nil) or (gps:speed_accuracy(gps:primary_sensor()) > gps_speedaccuracy_thresh)
-- get GPS innovations from ahrs -- get GPS innovations from ahrs
local gps_innov = Vector3f() local gps_innov = ahrs:get_vel_innovations_and_variances_for_source(3)
local gps_var = Vector3f()
gps_innov, gps_var = ahrs:get_vel_innovations_and_variances_for_source(3)
local gps_innov_over_threshold = (gps_innov == nil) or (gps_innov:z() == 0.0) or (math.abs(gps_innov:z()) > gps_innov_thresh) local gps_innov_over_threshold = (gps_innov == nil) or (gps_innov:z() == 0.0) or (math.abs(gps_innov:z()) > gps_innov_thresh)
-- automatic selection logic -- -- automatic selection logic --

View File

@ -14,14 +14,10 @@
-- SCR_USER3 holds the threshold for Non-GPS vertical speed innovation (about 0.3 is a good choice) -- SCR_USER3 holds the threshold for Non-GPS vertical speed innovation (about 0.3 is a good choice)
-- if both GPS speed accuracy <= SCR_USER2 and ExternalNav speed variance >= SCR_USER3, source1 will be used -- if both GPS speed accuracy <= SCR_USER2 and ExternalNav speed variance >= SCR_USER3, source1 will be used
-- otherwise source2 (T265) or source3 (optical flow) will be used based on rangefinder distance -- otherwise source2 (T265) or source3 (optical flow) will be used based on rangefinder distance
-- luacheck: only 0
---@diagnostic disable: need-check-nil ---@diagnostic disable: need-check-nil
---@diagnostic disable: cast-local-type
local rangefinder_rotation = 25 -- check downward (25) facing lidar local rangefinder_rotation = 25 -- check downward (25) facing lidar
local source_prev = 0 -- previous source, defaults to primary source local source_prev = 0 -- previous source, defaults to primary source
local sw_source_prev = -1 -- previous source switch position
local sw_auto_pos_prev = -1 -- previous auto source switch position local sw_auto_pos_prev = -1 -- previous auto source switch position
local auto_switch = false -- true when auto switching between sources is active local auto_switch = false -- true when auto switching between sources is active
local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value
@ -83,9 +79,7 @@ function update()
local gps_usable = (gps_speed_accuracy ~= nil) and (gps_speed_accuracy <= gps_usable_accuracy) local gps_usable = (gps_speed_accuracy ~= nil) and (gps_speed_accuracy <= gps_usable_accuracy)
-- get external nav innovations from ahrs -- get external nav innovations from ahrs
local extnav_innov = Vector3f() local extnav_innov = ahrs:get_vel_innovations_and_variances_for_source(6)
local extnav_var = Vector3f()
extnav_innov, extnav_var = ahrs:get_vel_innovations_and_variances_for_source(6)
local extnav_over_threshold = (extnav_innov == nil) or (extnav_innov:z() == 0.0) or (math.abs(extnav_innov:z()) > extnav_innov_thresh) local extnav_over_threshold = (extnav_innov == nil) or (extnav_innov:z() == 0.0) or (math.abs(extnav_innov:z()) > extnav_innov_thresh)
-- get rangefinder distance -- get rangefinder distance

View File

@ -140,7 +140,7 @@ function update()
-- first get position offset (cumulative effect of velocity offsets) -- first get position offset (cumulative effect of velocity offsets)
local pos_offset_NED, _, _ = poscontrol:get_posvelaccel_offset() local pos_offset_NED, _, _ = poscontrol:get_posvelaccel_offset()
if pos_offset_NED == nil then if pos_offset_NED == nil then
print_warning("unable to get position offset") print("unable to get position offset")
pos_offset_NED = Vector3f() pos_offset_NED = Vector3f()
end end
-- test velocity offsets in m/s in NED frame -- test velocity offsets in m/s in NED frame

View File

@ -5,8 +5,6 @@
local PARAM_TABLE_KEY = 72 local PARAM_TABLE_KEY = 72
local PARAM_TABLE_PREFIX = "DEPL_" local PARAM_TABLE_PREFIX = "DEPL_"
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
---@diagnostic disable: missing-parameter
---@diagnostic disable: param-type-mismatch ---@diagnostic disable: param-type-mismatch
-- bind a parameter to a variable -- bind a parameter to a variable
@ -69,7 +67,7 @@ function update_state()
gcs:send_text(MAV_SEVERITY.INFO, "DEPL: deploying") gcs:send_text(MAV_SEVERITY.INFO, "DEPL: deploying")
vehicle:set_mode(MODE_LAND) vehicle:set_mode(MODE_LAND)
arming:arm_force() arming:arm_force()
if not vehicle:get_mode() == MODE_LAND or not arming:is_armed() then if (vehicle:get_mode() ~= MODE_LAND) or not arming:is_armed() then
gcs:send_text(MAV_SEVERITY.INFO, "DEPL: Arming failed") gcs:send_text(MAV_SEVERITY.INFO, "DEPL: Arming failed")
return return
end end

View File

@ -17,8 +17,6 @@
For this test we'll use sensor ID 17 (0x71), For this test we'll use sensor ID 17 (0x71),
Note: 17 is the index, 0x71 is the actual ID Note: 17 is the index, 0x71 is the actual ID
--]] --]]
-- luacheck: only 0
---@diagnostic disable: param-type-mismatch
local loop_time = 1000 -- number of ms between runs local loop_time = 1000 -- number of ms between runs
@ -45,7 +43,7 @@ function wrap_360(angle)
end end
function prep_5bits(num) function prep_5bits(num)
local res = 0 local res
local abs_num = math.floor(math.abs(num) + 0.5) local abs_num = math.floor(math.abs(num) + 0.5)
if abs_num < 10 then if abs_num < 10 then
res = abs_num << 1 res = abs_num << 1
@ -61,7 +59,7 @@ function prep_5bits(num)
end end
function wp_pack(index, distance, bearing, xtrack) function wp_pack(index, distance, bearing, xtrack)
local wp_dword = uint32_t() local wp_dword
wp_dword = math.min(index,WP_LIMIT_COUNT) 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 | 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 wp_dword = wp_dword | prep_5bits(math.min(xtrack,WP_LIMIT_XTRACK)) << WP_OFFSET_XTRACK
@ -97,16 +95,13 @@ function update_wp_info()
end end
function update() function update()
local gps_status = gps.status(0,0)
if not update_wp_info() then if not update_wp_info() then
return update, loop_time return update, loop_time
end end
local sensor_id = 0x71 local sensor_id = 0x71
local wp_dword = uint32_t() local wp_dword = wp_pack(wp_index, wp_distance, wp_bearing, wp_xtrack)
wp_dword = wp_pack(wp_index, wp_distance, wp_bearing, wp_xtrack)
frsky_sport:sport_telemetry_push(sensor_id, 0x10, 0x5009, wp_dword) frsky_sport:sport_telemetry_push(sensor_id, 0x10, 0x5009, wp_dword)
return update, loop_time return update, loop_time

View File

@ -5,8 +5,6 @@
generators. It monitors battery voltage and controls the throttle generators. It monitors battery voltage and controls the throttle
of the generator to maintain a target voltage using a PI controller of the generator to maintain a target voltage using a PI controller
--]] --]]
-- luacheck: only 0
---@diagnostic disable: need-check-nil ---@diagnostic disable: need-check-nil
---@diagnostic disable: param-type-mismatch ---@diagnostic disable: param-type-mismatch
@ -63,7 +61,6 @@ GENCTL_VOLT_TARG = bind_add_param('VOLT_TARG', 11, 0)
GENCTL_SLEW_RATE = bind_add_param('SLEW_RATE', 12, 100) GENCTL_SLEW_RATE = bind_add_param('SLEW_RATE', 12, 100)
local MAV_SEVERITY_INFO = 6 local MAV_SEVERITY_INFO = 6
local MAV_SEVERITY_NOTICE = 5
local MAV_SEVERITY_EMERGENCY = 0 local MAV_SEVERITY_EMERGENCY = 0
local switch = nil local switch = nil

View File

@ -1,7 +1,5 @@
-- mission editing demo lua script. -- mission editing demo lua script.
-- by Buzz 2020 -- by Buzz 2020
-- luacheck: only 0
---@diagnostic disable: param-type-mismatch ---@diagnostic disable: param-type-mismatch
---@diagnostic disable: undefined-field ---@diagnostic disable: undefined-field
@ -202,7 +200,7 @@ function stage7 ()
-- fiurst time in , there's no mission, lets throw a few wps in to play with later.. -- fiurst time in , there's no mission, lets throw a few wps in to play with later..
-- change copy of last item slightly, for giggles, and append as a new item -- change copy of last item slightly, for giggles, and append as a new item
if (mission:num_commands() == 1) then if (mission:num_commands() == 1) then
for x = 1, 10 do for _ = 1, 10 do
m:command(16) -- 16 = normal WAYPOINT m:command(16) -- 16 = normal WAYPOINT
m:x(m:x()+math.random(-10000,10000)) -- add something random m:x(m:x()+math.random(-10000,10000)) -- add something random
m:y(m:y()+math.random(-10000,10000)) m:y(m:y()+math.random(-10000,10000))

View File

@ -1,6 +1,5 @@
-- Example of loading a mission from the SD card using Scripting -- Example of loading a mission from the SD card using Scripting
-- Would be trivial to select a mission based on scripting params or RC switch -- Would be trivial to select a mission based on scripting params or RC switch
-- luacheck: only 0
--Copy this "mission-load.lua" script to the "scripts" directory of the simulation or autopilot's SD card. --Copy this "mission-load.lua" script to the "scripts" directory of the simulation or autopilot's SD card.
--The "mission1.txt" file containing the mission items should be placed in the directory above the "scripts" directory. --The "mission1.txt" file containing the mission items should be placed in the directory above the "scripts" directory.
--In case of placing it on SD Card, mission1.txt file should be placed in the APM directory root. --In case of placing it on SD Card, mission1.txt file should be placed in the APM directory root.
@ -23,12 +22,11 @@ local function read_mission(file_name)
local index = 0 local index = 0
local fail = false local fail = false
while true and not fail do while true and not fail do
local data = {}
local line = file:read() local line = file:read()
if not line then if not line then
break break
end end
local ret, _, seq, curr, frame, cmd, p1, p2, p3, p4, x, y, z, autocont = string.find(line, "^(%d+)%s+(%d+)%s+(%d+)%s+(%d+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+(%d+)") local ret, _, seq, _--[[ curr ]], frame, cmd, p1, p2, p3, p4, x, y, z, _--[[ autocont ]] = string.find(line, "^(%d+)%s+(%d+)%s+(%d+)%s+(%d+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+([-.%d]+)%s+(%d+)")
if not ret then if not ret then
fail = true fail = true
break break

View File

@ -1,7 +1,6 @@
-- warn the user if wind speed exceeds a threshold, failsafe if a second threshold is exceeded -- warn the user if wind speed exceeds a threshold, failsafe if a second threshold is exceeded
-- note that this script is only intended to be run on ArduPlane -- note that this script is only intended to be run on ArduPlane
-- luacheck: only 0
-- tuning parameters -- tuning parameters
local warn_speed = 10 -- metres/second local warn_speed = 10 -- metres/second
@ -11,26 +10,26 @@ local warning_interval_ms = uint32_t(15000) -- send user message every 15s
local warning_last_sent_ms = uint32_t() -- time we last sent a warning message to the user local warning_last_sent_ms = uint32_t() -- time we last sent a warning message to the user
function update() function update()
local wind = ahrs:wind_estimate() -- get the wind estimate local wind = ahrs:wind_estimate() -- get the wind estimate
if wind then if wind then
-- make a 2D wind vector -- make a 2D wind vector
wind_xy = Vector2f() wind_xy = Vector2f()
wind_xy:x(wind:x()) wind_xy:x(wind:x())
wind_xy:y(wind:y()) wind_xy:y(wind:y())
speed = wind_xy:length() -- compute the wind speed speed = wind_xy:length() -- compute the wind speed
if speed > failsafe_speed then if speed > failsafe_speed then
gcs:send_text(0, "Wind failsafe at " .. speed .. " metres/second") gcs:send_text(0, "Wind failsafe at " .. speed .. " metres/second")
vehicle:set_mode(11) -- FIXME: should be an enum. 11 is RTL. vehicle:set_mode(11) -- FIXME: should be an enum. 11 is RTL.
return return
end end
if speed > warn_speed then if speed > warn_speed then
if millis() - warning_last_sent_ms > warning_interval_ms then if millis() - warning_last_sent_ms > warning_interval_ms then
gcs:send_text(4, "Wind warning at " .. speed .. " metres/second") gcs:send_text(4, "Wind warning at " .. speed .. " metres/second")
warning_last_sent_ms = millis() warning_last_sent_ms = millis()
end end
end end
end end
return update, 1000 return update, 1000
end end
return update, 1000 return update, 1000

View File

@ -1,12 +1,8 @@
-- support follow in GUIDED mode in plane -- support follow in GUIDED mode in plane
-- luacheck: only 0
---@diagnostic disable: cast-local-type
local PARAM_TABLE_KEY = 11 local PARAM_TABLE_KEY = 11
local PARAM_TABLE_PREFIX = "GFOLL_" local PARAM_TABLE_PREFIX = "GFOLL_"
local MODE_MANUAL = 0
local MODE_GUIDED = 15 local MODE_GUIDED = 15
local ALT_FRAME_ABSOLUTE = 0 local ALT_FRAME_ABSOLUTE = 0
@ -29,13 +25,10 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add p
GFOLL_ENABLE = bind_add_param('ENABLE', 1, 0) GFOLL_ENABLE = bind_add_param('ENABLE', 1, 0)
-- current target -- current target
local target_pos = Location() local target_pos
local current_pos = Location() local current_pos
local target_velocity = Vector3f()
local target_heading = 0.0
-- other state -- other state
local vehicle_mode = MODE_MANUAL
local have_target = false local have_target = false
-- check key parameters -- check key parameters
@ -89,8 +82,7 @@ function update_target()
end end
have_target = true have_target = true
target_pos, target_velocity = follow:get_target_location_and_velocity_ofs() target_pos = follow:get_target_location_and_velocity_ofs()
target_heading = follow:get_target_heading_deg()
end end
-- main update function -- main update function

View File

@ -21,7 +21,7 @@ function update()
if pwm7 and pwm7 > 1800 then -- check if RC input 7 has moved high if pwm7 and pwm7 > 1800 then -- check if RC input 7 has moved high
local mode = vehicle:get_mode() -- get current mode local mode = vehicle:get_mode() -- get current mode
if not sent_target then -- if we haven't sent the target yet if not sent_target then -- if we haven't sent the target yet
if not (mode == copter_guided_mode_num) then -- change to guided mode if mode ~= copter_guided_mode_num then -- change to guided mode
vehicle:set_mode(copter_guided_mode_num) vehicle:set_mode(copter_guided_mode_num)
else else
local above_home = ahrs:get_home() -- get home location local above_home = ahrs:get_home() -- get home location
@ -33,7 +33,7 @@ function update()
else else
-- change to land mode when within 2m of home -- change to land mode when within 2m of home
if not (mode == copter_land_mode_num) then if mode ~= copter_land_mode_num then
local home = ahrs:get_home() local home = ahrs:get_home()
local curr_loc = ahrs:get_location() local curr_loc = ahrs:get_location()
if home and curr_loc then if home and curr_loc then

View File

@ -6,7 +6,7 @@ local new_target= nil
function update() function update()
if not (vehicle:get_mode() == 4) then if vehicle:get_mode() ~= 4 then
gcs:send_text(0, "not in Guided") gcs:send_text(0, "not in Guided")
return update, 1000 return update, 1000
end end