From 2e89a0f9a07c5686bd9cabbc35072871cf738d3a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 Nov 2024 10:30:29 +0900 Subject: [PATCH] AP_Scripting: ahrs/ekf origin script promoted to an applet --- .../AP_Scripting/applets/ahrs-set-origin.lua | 100 ++++++++++++++++++ .../AP_Scripting/applets/ahrs-set-origin.md | 15 +++ .../AP_Scripting/examples/ahrs-set-origin.lua | 23 ---- 3 files changed, 115 insertions(+), 23 deletions(-) create mode 100644 libraries/AP_Scripting/applets/ahrs-set-origin.lua create mode 100644 libraries/AP_Scripting/applets/ahrs-set-origin.md delete mode 100644 libraries/AP_Scripting/examples/ahrs-set-origin.lua diff --git a/libraries/AP_Scripting/applets/ahrs-set-origin.lua b/libraries/AP_Scripting/applets/ahrs-set-origin.lua new file mode 100644 index 0000000000000..a2f25fda87d1a --- /dev/null +++ b/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() \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/ahrs-set-origin.md b/libraries/AP_Scripting/applets/ahrs-set-origin.md new file mode 100644 index 0000000000000..27ab6fc5c6273 --- /dev/null +++ b/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 diff --git a/libraries/AP_Scripting/examples/ahrs-set-origin.lua b/libraries/AP_Scripting/examples/ahrs-set-origin.lua deleted file mode 100644 index 37ec058d2cd37..0000000000000 --- a/libraries/AP_Scripting/examples/ahrs-set-origin.lua +++ /dev/null @@ -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() \ No newline at end of file