AP_Scripting: ahrs/ekf origin script promoted to an applet

libraries/AP_Scripting/applets/ahrs-set-origin.lua

@@ -0,0 +1,100 @@
+-- Sets the AHRS/EKF origin to a specified Location
+--
+-- Parameter descriptions
+-- AHRS_ORIG_LAT : AHRS Origin Latitude (in degrees)
+-- AHRS_ORIG_LON : AHRS Origin Longitude (in degrees)
+-- AHRS_ORIGIN_ALT : AHRS Origin Altitude (in meters above sea level)
+--
+-- How to use
+-- 1. Install this script on the flight controller
+-- 2. Set AHRS_ORIG_LAT, AHRS_ORIG_LON, AHRS_ORIG_ALT to the desired location
+-- 3. A message should appear on the messages screen when the AHRS/EKF origin has been set and the vehicle will most often then appear on the map
+
+local PARAM_TABLE_KEY = 87
+PARAM_TABLE_PREFIX = "AHRS_ORIG_"
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+local SEND_TEXT_PREFIX = "ahrs-set-origin: "
+
+-- bind a parameter to a variable
+function bind_param(name)
+  local p = Parameter()
+  assert(p:init(name), string.format('could not find %s parameter', name))
+  return p
+end
+
+-- add a parameter and bind it to a variable
+local function bind_add_param(name, idx, default_value)
+  assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+  return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- add param table
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), SEND_TEXT_PREFIX .. 'could not add param table')
+
+--[[
+  // @Param: AHRS_ORIG_LAT
+  // @DisplayName: AHRS/EKF Origin Latitude
+  // @Description: AHRS/EKF origin will be set to this latitude if not already set
+  // @Range: -180 180
+  // @User: Standard
+--]]
+local AHRS_ORIG_LAT = bind_add_param('LAT', 1, 0)
+
+--[[
+  // @Param: AHRS_ORIG_LON
+  // @DisplayName: AHRS/EKF Origin Longitude
+  // @Description: AHRS/EKF origin will be set to this longitude if not already set
+  // @Range: -180 180
+  // @User: Standard
+--]]
+local AHRS_ORIG_LON = bind_add_param('LON', 2, 0)
+
+--[[
+  // @Param: AHRS_ORIG_ALT
+  // @DisplayName: AHRS/EKF Origin Altitude
+  // @Description: AHRS/EKF origin will be set to this altitude (in meters above sea level) if not already set
+  // @Range: 0 10000
+  // @User: Standard
+--]]
+local AHRS_ORIG_ALT = bind_add_param('ALT', 3, 0)
+
+-- print welcome message
+gcs:send_text(MAV_SEVERITY.INFO, SEND_TEXT_PREFIX .. "started")
+
+function update()
+
+    -- wait for AHRS to be initialised
+    if not ahrs:initialised() then
+        return update, 5000
+    end
+
+    -- exit if AHRS/EKF origin has already been set
+    if ahrs:get_origin() then
+        gcs:send_text(MAV_SEVERITY.WARNING, SEND_TEXT_PREFIX .. "EKF origin already set")
+        return
+    end
+
+    -- return if parameters have not been set
+    if AHRS_ORIG_LAT:get() == 0 and AHRS_ORIG_LON:get() == 0 and AHRS_ORIG_ALT == 0 then
+        -- try again in 5 seconds
+        return update, 5000
+    end
+
+    -- create new origin
+    local location = Location()
+    location:lat(math.floor(AHRS_ORIG_LAT:get() * 10000000.0))
+    location:lng(math.floor(AHRS_ORIG_LON:get() * 10000000.0))
+    location:alt(math.floor(AHRS_ORIG_ALT:get() * 100.0))
+
+    -- attempt to send origin
+    if ahrs:set_origin(location) then
+        gcs:send_text(MAV_SEVERITY.INFO, string.format(SEND_TEXT_PREFIX .. "origin set Lat:%.7f Lon:%.7f Alt:%.1f", AHRS_ORIG_LAT:get(), AHRS_ORIG_LON:get(), AHRS_ORIG_ALT:get()))
+    else
+        gcs:send_text(MAV_SEVERITY.WARNING, SEND_TEXT_PREFIX .. "failed to set origin")
+    end
+
+    -- return and do not try again
+    return
+end
+
+return update()

libraries/AP_Scripting/applets/ahrs-set-origin.md

@@ -0,0 +1,15 @@
+# AHRS set origin
+
+Sets the AHRS/EKF origin to a specified Location
+
+## Parmeter Descriptions
+
+-- AHRS_ORIG_LAT : AHRS Origin Latitude (in degrees)
+-- AHRS_ORIG_LON : AHRS Origin Longitude (in degrees)
+-- AHRS_ORIGIN_ALT : AHRS Origin Altitude (in meters above sea level)
+
+## How to use
+
+Install this script on the flight controller
+Set AHRS_ORIG_LAT, AHRS_ORIG_LON, AHRS_ORIG_ALT to the desired location
+A message should appear on the messages screen when the AHRS/EKF origin has been set and the vehicle will most often then appear on the map

libraries/AP_Scripting/examples/ahrs-set-origin.lua

@@ -1,23 +0,0 @@
--- example script for using "set_origin()"" and "initialised()"
--- sets the ekf origin if not already set
-
-
-function update ()
-
-    if not ahrs:initialised() then
-        return update, 5000
-    end
-    
-    origin = assert(not ahrs:get_origin(),"Refused to set EKF origin - already set")
-    location = Location() location:lat(-353632640) location:lng(1491652352) location:alt(58409)
-    
-    if ahrs:set_origin(location) then
-        gcs:send_text(6, string.format("Origin Set - Lat:%.7f Long:%.7f Alt:%.1f", location:lat()/10000000, location:lng()/10000000, location:alt()/100))
-    else
-        gcs:send_text(0, "Refused to set EKF origin")
-    end
-
-    return
-end
-
-return update()