diff --git a/libraries/AP_Scripting/examples/mag_heading.lua b/libraries/AP_Scripting/examples/mag_heading.lua new file mode 100644 index 0000000000..d6c1e24c01 --- /dev/null +++ b/libraries/AP_Scripting/examples/mag_heading.lua @@ -0,0 +1,50 @@ +--[[ + send magnetic heading in degrees as NAMED_VALUE_FLOAT[MAG_HEAD] and NAMED_VALUE_FLOAT[MAG_GCRS] +--]] + +local RATE_HZ = 2 + +-- bind a parameter to a variable given +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +COMPASS_DEC = bind_param("COMPASS_DEC") + +function wrap_360(angle) + local res = angle % 360 + if res < 0 then + res = res + 360 + end + return res +end + +function update() + local yaw_deg = wrap_360(math.deg(ahrs:get_yaw() - COMPASS_DEC:get())) + local gspd = ahrs:groundspeed_vector() + local gcrs_deg = wrap_360(math.deg(math.atan(gspd:y(), gspd:x()) - COMPASS_DEC:get())) + gcs:send_named_float("MAG_HEAD", yaw_deg) + gcs:send_named_float("MAG_GCRS", gcrs_deg) +end + +gcs:send_text(0, "MagHeading loaded") + +local MAV_SEVERITY_ERROR = 3 + +-- wrapper around update(). This calls update() and if update faults +-- then an error is displayed, but the script is not stopped +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY_ERROR, "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, math.floor(1000 / RATE_HZ) +end + +-- start running update loop +return protected_wrapper()