/
AutoChokeSaveWP.lua
136 lines (117 loc) · 5.27 KB
/
AutoChokeSaveWP.lua
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
-------------------------------------------------
---- AutoChokeSaveWP ArduPilot Lua script -------
-- --
-- Sets choke for cold/warm start by polling --
-- ignition and starter switch states and --
-- setting the choke for a cold start, then --
-- slowly backs the choke off as the engine --
-- presumably warms up. --
-- --
-- If the vehicle is armed, the starter switch --
-- becomes a save waypoint switch --
-- --
--------------- Yuri -- Aug 2021 ----------------
-------------------------------------------------
-------- USER EDITABLE GLOBALS --------
local AUTO = 10 -- rover auto mode number
local WAYPOINT_COMMAND = 16 -- normal waypoint command number
local IGNITION_CHANNEL = 5 -- I used a reversed channel for this
local IGNITION_THRESHOLD = 2000 -- pwm threshold to determine state
local STARTER_RELAY = 4 -- starter relay channel
local STARTER_CHANNEL = 7 -- starter trigger channel
local STARTER_THRESHOLD = 1500 -- pwm threshold to determine state
local CHOKE_SERVO_FN = 94 -- recommend 94-109 (SERVOx_FUNCTION = Script <1-16>
local CHOKE_MIN = 982 -- min choke pwm
local CHOKE_MAX = 1469 -- max choke pwm
local CHOKE_TIMEOUT = 4500 -- (ms) incrementally decrease choke during this amount of time (after engine start)
local FREQUENCY = 50 -- (ms) how often to run this script (100 or maybe a bit less)
local VERBOSE_MODE = 1 -- 0 to suppress all GCS messages, 1 for status updates, 2 for additional debug messages
-------- END USER EDITABLE GLOBALS --------
local lastStarterPwm = rc:get_pwm(STARTER_CHANNEL)
local coldStart = false
local cranking = false
local crankingComplete = false
local timer = 0
-- initialize with choke off
SRV_Channels:set_output_pwm(CHOKE_SERVO_FN, CHOKE_MIN)
function autoChoke()
if (coldStart and timer > millis()) then
local choke_pwm_diff = math.floor((CHOKE_MAX - CHOKE_MIN) * (timer:tofloat() - millis():tofloat()) / CHOKE_TIMEOUT)
local choke_pwm = CHOKE_MIN + choke_pwm_diff
SRV_Channels:set_output_pwm(CHOKE_SERVO_FN, choke_pwm)
return
end
if (coldStart and crankingComplete) then
SRV_Channels:set_output_pwm(CHOKE_SERVO_FN, CHOKE_MIN)
coldStart = false
crankingComplete = false
if (VERBOSE_MODE > 0) then gcs:send_text(6, "Cold start complete, choke OFF") end
return
end
local ignitionPwm = rc:get_pwm(IGNITION_CHANNEL)
local starterPwm = rc:get_pwm(STARTER_CHANNEL)
if starterPwm == lastStarterPwm then return end
lastStarterPwm = starterPwm
if (starterPwm < STARTER_THRESHOLD and ignitionPwm > IGNITION_THRESHOLD) then
coldStart = not coldStart
if (coldStart) then
SRV_Channels:set_output_pwm(CHOKE_SERVO_FN, CHOKE_MAX)
crankingComplete = false
if (VERBOSE_MODE > 0) then gcs:send_text(6, "Cold start, choke ON") end
return
end
SRV_Channels:set_output_pwm(CHOKE_SERVO_FN, CHOKE_MIN)
if (VERBOSE_MODE > 0) then gcs:send_text(6, "Warm start, choke OFF") end
return
end
--if (coldStart and starterPwm < STARTER_THRESHOLD and ignitionPwm < IGNITION_THRESHOLD) then
if (starterPwm < STARTER_THRESHOLD and ignitionPwm < IGNITION_THRESHOLD) then
relay:off(STARTER_RELAY)
cranking = true
if (VERBOSE_MODE > 1) then gcs:send_text(6, "Cranking engine") end
return
end
if (cranking and starterPwm > STARTER_THRESHOLD and ignitionPwm < IGNITION_THRESHOLD) then
relay:on(STARTER_RELAY)
cranking = false
crankingComplete = true
if (coldStart) then
timer = millis() + CHOKE_TIMEOUT
end
if (VERBOSE_MODE > 1) then gcs:send_text(6, "Cranking complete") end
return
end
end
function waypointSaver()
local starterPwm = rc:get_pwm(STARTER_CHANNEL)
if starterPwm == lastStarterPwm then return end
lastStarterPwm = starterPwm
if (starterPwm < STARTER_THRESHOLD) then
if (not ahrs:healthy()) then
if (VERBOSE_MODE > 0) then gcs:send_text(5, "WP not saved, AHRS unhealthy") end
return
end
local location = ahrs:get_position()
if (location == nil) then
if (VERBOSE_MODE > 0) then gcs:send_text(5, "WP not saved, location unknown") end
return
end
local template = mission:get_item(0)
template:command(WAYPOINT_COMMAND)
template:x(location:lat())
template:y(location:lng())
template:z(location:alt())
mission:set_item(mission:num_commands(), template)
if (VERBOSE_MODE > 0) then gcs:send_text(6, "Saved Waypoint: " .. mission:num_commands()) end
end
end
function update()
if (arming:is_armed() and vehicle:get_mode() ~= AUTO) then
waypointSaver()
return update, FREQUENCY
end
autoChoke()
return update, FREQUENCY
end
if (VERBOSE_MODE > 0) then gcs:send_text(6, "AutoChoke: Choke OFF") end
return update()