Spent a few days learning LUA and coding up a script to look after a flight. Here is the result. It has taken a while to go from "syntax errors" to "bad argument" errors to clean code for the FSM
. Keeping the units and signs consistent was also something I was paranoid about getting correct.Code has been tested and should run well on a flight vehicle
Code: Select all
-- This script is the main flight script for a rocket
-- SRC 22JAN2021 v1.0
-- SCRIPT FUNCTIONS
-- 1) Initialise
-- 2) Cycle canards as a servo test for 20s
-- 3) Await for boost detection (accel for > 1s)
-- 4) Switch flight to STABILIZE when drag decelerates airframe
-- 5) Switch to MANUAL when tilt > 45deg or time > 30s
-- 6) Loop forever
-- Note: This routine assumes there will be another script for detecting breakwire break and putting
-- the vehicle in MANU mode at deployment
local state = 1 -- state machine variable
local drag = 0.2 -- decel drag to trigger STAB (G)
local boost = 2 -- accel to sense boost (G)
local tick = 0 -- time counter, one tick = 20s
local STAB_mode = 2 -- STABILIZE mode in Plane
local MANU_mode = 0 -- MANUAL mode in Plane
local period = 2 -- sweep rate for servo test
local z -- acceleration variable
local accel -- for acquiring 3D vector
function statemachine()
if state == 1 then -- initialise
if tick == 0 then
gcs:send_text(0,"LUA:Servo Sweeps")
end
local t = 0.001 * millis():tofloat()
local pi = 3.1416
local output = math.sin(pi * t * period * 2.0 )
local pwm = math.floor(1500 + 17 * output) -- 17 should give +/- 5deg sweep maximum
srv_channels:set_output_pwm( 4, pwm)
srv_channels:set_output_pwm(79, pwm)
srv_channels:set_output_pwm(80, pwm)
tick = tick + 1
if tick > (50 * 20) then -- 20s is up
state = 2
tick = 0
vehicle:set_mode(MANU_mode)
gcs:send_text(0,"LUA:Waiting for Boost")
end
elseif state == 2 then -- wait for boost detect
accel = ahrs:get_accel()
z = accel:z() -- accel in m/s^2
if z < ((-1 - boost) * 9.8) then -- accelerating up, 1 + boost G
tick = tick + 1
if (tick > 50) then -- one second of boost
state = 3
tick = 0
gcs:send_text(0,"LUA:Boost Detected")
end
end
elseif state == 3 then -- wait for decel (end of boost)
accel = ahrs:get_accel()
z = accel:z() -- accel in m/s^2
if z > (drag * 9.8) then -- decelerating due to drag only, freefall otherwise ;-)
tick = tick + 1
if tick > 4 then -- over 80ms of coast delay, contiguous
gcs:send_text(0,"LUA:Coast Detected, STAB Set")
tick = 0
vehicle:set_mode(STAB_mode) -- no guts, no glory
state = 4
gcs:send_text(0, "Waiting")
end
else tick = 0 -- debouncing
end
elseif state == 4 then -- delay here a little, helps with testing
if tick >4 then
tick = 0
state = 5
else
tick = tick + 1
end
elseif state == 5 then -- already in STAB, now look to exit to MANU
tick = tick + 1
if (tick > 1000) or (math.abs(ahrs:get_roll()) > 0.78) or (math.abs(ahrs:get_pitch()) > 0.78) then -- angles in radians
-- exit stabilisation
-- rocket pitching over towards ground or time exceeds 20s
tick = 0
vehicle:set_mode(MANU_mode)
gcs:send_text(0,"LUA:Flight Complete")
state = 6
end
elseif state == 6 then -- loop forever
state = 6
end
return statemachine, 20 -- reschedule every 20ms
end
return statemachine, 10000 -- wait 10s before starting this script
seriously cool 
