Skip to content

Commit 1e9a102

Browse files
andyp1perandyp1per
committed
AP_Scripting: AP calibration menu
1 parent 127b4d0 commit 1e9a102

File tree

1 file changed

+157
-0
lines changed

1 file changed

+157
-0
lines changed

‎libraries/AP_Scripting/applets/crsf-calibrate.lua

+157
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
--[[
2+
-- A script to perform ArduPilot calibrations using a custom CRSF menu
3+
-- copy this script to the autopilot's "scripts" directory
4+
--]]
5+
6+
SCRIPT_NAME = "ArduPilot Calibration"
7+
SCRIPT_NAME_SHORT = "Calibration"
8+
SCRIPT_VERSION = "0.1"
9+
10+
MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
11+
CRSF_EVENT = {PARAMETER_READ=1, PARAMETER_WRITE=2}
12+
CRSF_PARAM_TYPE = {
13+
UINT8 = 0, -- deprecated
14+
INT8 = 1, -- deprecated
15+
UINT16 = 2, -- deprecated
16+
INT16 = 3, -- deprecated
17+
FLOAT = 8,
18+
TEXT_SELECTION = 9,
19+
STRING = 10,
20+
FOLDER = 11,
21+
INFO = 12,
22+
COMMAND = 13,
23+
}
24+
25+
CRSF_COMMAND_STATUS = {
26+
READY = 0, -- --> feedback
27+
START = 1, -- <-- input
28+
PROGRESS = 2, -- --> feedback
29+
CONFIRMATION_NEEDED = 3, -- --> feedback
30+
CONFIRM = 4, -- <-- input
31+
CANCEL = 5, -- <-- input
32+
POLL = 6 -- <-- input
33+
}
34+
35+
MAV_CMD_PREFLIGHT_CALIBRATION = 241
36+
MAV_CMD_DO_START_MAG_CAL = 42424
37+
MAV_CMD_DO_ACCEPT_MAG_CAL = 42425
38+
MAV_CMD_DO_CANCEL_MAG_CAL = 42426
39+
40+
-- create a CRSF menu float item
41+
function create_float_entry(name, value, min, max, default, dpoint, step, unit)
42+
return string.pack(">BzllllBlz", CRSF_PARAM_TYPE.FLOAT, name, value, min, max, default, dpoint, step, unit)
43+
end
44+
45+
-- create a CRSF menu text selection item
46+
function create_text_entry(name, options, value, min, max, default, unit)
47+
return string.pack(">BzzBBBBz", CRSF_PARAM_TYPE.TEXT_SELECTION, name, options, value, min, max, default, unit)
48+
end
49+
50+
-- create a CRSF menu string item
51+
function create_string_entry(name, value, max)
52+
return string.pack(">BzzB", CRSF_PARAM_TYPE.STRING, name, value, max)
53+
end
54+
55+
-- create a CRSF menu info item
56+
function create_info_entry(name, info)
57+
return string.pack(">Bzz", CRSF_PARAM_TYPE.INFO, name, info)
58+
end
59+
60+
-- create a CRSF command entry
61+
function create_command_entry(name, status, timeout, info)
62+
timeout = timeout or 10 -- 1s
63+
return string.pack(">BzBBz", CRSF_PARAM_TYPE.COMMAND, name, status, timeout, info)
64+
end
65+
66+
local compass_command, accel_command, gyro_command, forceaccel_command, forcecompass_command, ahrs_command
67+
68+
local menu = crsf:add_menu('Calibrations')
69+
local compass_param = create_command_entry("Calibrate Compass", CRSF_COMMAND_STATUS.READY, 50, "Command")
70+
local accel_param = create_command_entry("Calibrate Accels", CRSF_COMMAND_STATUS.READY, 50, "Command")
71+
local gyro_param = create_command_entry("Calibrate Gyros", CRSF_COMMAND_STATUS.READY, 50, "Command")
72+
local forceaccel_param = create_command_entry("Forcecal Accels", CRSF_COMMAND_STATUS.READY, 50, "Command")
73+
local forcecompass_param = create_command_entry("Forcecal Compass", CRSF_COMMAND_STATUS.READY, 50, "Command")
74+
local ahrs_param = create_command_entry("Trim AHRS", CRSF_COMMAND_STATUS.READY, 50, "Command")
75+
76+
if menu ~= nil then
77+
compass_command = menu:add_parameter(compass_param)
78+
accel_command = menu:add_parameter(accel_param)
79+
gyro_command = menu:add_parameter(gyro_param)
80+
forceaccel_command = menu:add_parameter(forceaccel_param)
81+
forcecompass_command = menu:add_parameter(forcecompass_param)
82+
ahrs_command = menu:add_parameter(ahrs_param)
83+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Loaded CRSF calibration menu"))
84+
end
85+
86+
local calibration_running = false
87+
88+
function update()
89+
local param, payload, events = crsf:get_menu_event(CRSF_EVENT.PARAMETER_WRITE)
90+
if (events & CRSF_EVENT.PARAMETER_WRITE) ~= 0 then
91+
if compass_command ~= nil and param == compass_command:id() then
92+
local command_action = string.unpack(">B", payload)
93+
if command_action == CRSF_COMMAND_STATUS.START then -- start calibration
94+
-- mag cal
95+
if calibration_running then
96+
gcs:run_command_int(MAV_CMD_DO_CANCEL_MAG_CAL, { p3 = 1 })
97+
end
98+
calibration_running = true
99+
gcs:run_command_int(MAV_CMD_DO_START_MAG_CAL, { p3 = 1 })
100+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Compass calibration running"))
101+
crsf:send_write_response(create_command_entry("Calibrate Compass", CRSF_COMMAND_STATUS.CONFIRMATION_NEEDED, 50, "Accept"))
102+
elseif command_action == CRSF_COMMAND_STATUS.CONFIRM then -- confirm acceptance
103+
gcs:run_command_int(MAV_CMD_DO_ACCEPT_MAG_CAL, { p3 = 1 })
104+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Compass calibration accepted"))
105+
crsf:send_write_response(compass_param)
106+
elseif command_action == CRSF_COMMAND_STATUS.CANCEL and calibration_running then
107+
gcs:run_command_int(MAV_CMD_DO_CANCEL_MAG_CAL, { p3 = 1 })
108+
gcs:send_text(MAV_SEVERITY.WARNING, "Calibration cancelled")
109+
calibration_running = false
110+
crsf:send_write_response(compass_param)
111+
end
112+
elseif accel_command ~= nil and param == accel_command:id() then
113+
local command_action = string.unpack(">B", payload)
114+
if command_action == CRSF_COMMAND_STATUS.START then
115+
-- accelcalsimple
116+
gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 4 })
117+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Accels calibrated"))
118+
crsf:send_write_response(accel_param)
119+
end
120+
elseif gyro_command ~= nil and param == gyro_command:id() then
121+
local command_action = string.unpack(">B", payload)
122+
if command_action == CRSF_COMMAND_STATUS.START then
123+
-- gyro cal
124+
gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p1 = 1 })
125+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Gyros calibrated"))
126+
crsf:send_write_response(gyro_param)
127+
end
128+
elseif forceaccel_command ~= nil and param == forceaccel_command:id() then
129+
local command_action = string.unpack(">B", payload)
130+
if command_action == CRSF_COMMAND_STATUS.START then
131+
-- forcecal accel
132+
gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 76 })
133+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Accels force calibrated"))
134+
crsf:send_write_response(forceaccel_param)
135+
end
136+
elseif forcecompass_command ~= nil and param == forcecompass_command:id() then
137+
local command_action = string.unpack(">B", payload)
138+
if command_action == CRSF_COMMAND_STATUS.START then
139+
-- forcecal compass
140+
gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p2 = 76 })
141+
gcs:send_text(MAV_SEVERITY.INFO, string.format("Compass force calibrated"))
142+
crsf:send_write_response(forcecompass_param)
143+
end
144+
elseif ahrs_command ~= nil and param == ahrs_command:id() then
145+
local command_action = string.unpack(">B", payload)
146+
if command_action == CRSF_COMMAND_STATUS.START then
147+
-- ahrs trim
148+
gcs:run_command_int(MAV_CMD_PREFLIGHT_CALIBRATION, { p5 = 2 })
149+
gcs:send_text(MAV_SEVERITY.INFO, string.format("AHRS trimmed"))
150+
crsf:send_write_response(ahrs_param)
151+
end
152+
end
153+
end
154+
return update, 100
155+
end
156+
157+
return update, 5000

0 commit comments

Comments
 (0)