mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-07 08:23:56 -04:00
AP_Scripting: fix remaining luacheck issues
This commit is contained in:
parent
95d4dc1a0d
commit
0dbbb87ce2
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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 = {}
|
||||||
|
@ -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
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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()
|
||||||
|
@ -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()
|
||||||
|
|
||||||
|
@ -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()
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
|
||||||
--[[
|
--[[
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
|
||||||
|
@ -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 --
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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))
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user