AP_AHRS: set location implementation

libraries/AP_AHRS/AP_AHRS.cpp

@@ -770,6 +770,39 @@ bool AP_AHRS::_get_location(Location &loc) const
     return false;
 }
 
+bool AP_AHRS::set_location(const Location &loc)
+{
+    bool readyToAccept {dcm.ready_to_accept_location()};
+#if HAL_NAVEKF2_AVAILABLE
+    const bool ek2Available {EKF2.get_enable() && initialised(EKFType::TWO)};
+    readyToAccept &= !ek2Available || EKF2.readyToAcceptLLH();
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+    const bool ek3Available {EKF3.get_enable() && initialised(EKFType::THREE)};
+    readyToAccept &= !ek3Available || EKF3.readyToAcceptLLH();
+#endif
+
+    // Z coordinate is not processed in the functions below
+    if (readyToAccept && loc.check_latlng()) {
+        bool dcmSucceed{dcm.set_location(loc)};
+        bool ek2Succeed{true};
+        bool ek3Succeed{true};
+#if HAL_NAVEKF2_AVAILABLE
+        if (ek2Available) {
+            ek2Succeed = EKF2.setLLH(loc);
+        }
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+        if (ek3Available) {
+            ek3Succeed = EKF3.setLLH(loc);
+        }
+#endif
+
+        return dcmSucceed && ek2Succeed && ek3Succeed;
+    }
+    return false;
+}
+
 // status reporting of estimated errors
 float AP_AHRS::get_error_rp(void) const
 {
@@ -2251,6 +2284,12 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
 
 // true if the AHRS has completed initialisation
 bool AP_AHRS::initialised(void) const
+{
+    return initialised(ekf_type());
+}
+
+// true if the given AHRS type has completed initialisation
+bool AP_AHRS::initialised(EKFType type) const
 {
     switch (ekf_type()) {
 #if AP_AHRS_DCM_ENABLED

libraries/AP_AHRS/AP_AHRS.h

@@ -91,6 +91,8 @@ class AP_AHRS {
     // get current location estimate
     bool get_location(Location &loc) const;
 
+    bool set_location(struct Location const &loc);
+
     // get latest altitude estimate above ground level in meters and validity flag
     bool get_hagl(float &hagl) const WARN_IF_UNUSED;
 
@@ -742,6 +744,12 @@ class AP_AHRS {
     // update roll_sensor, pitch_sensor and yaw_sensor
     void update_cd_values(void);
 
+    // return origin for a specified EKF type
+    bool get_origin(EKFType type, Location &ret) const;
+
+    // true if the given AHRS type has completed initialisation
+    bool initialised(EKFType type) const;
+
     // helper trig variables
     float _cos_roll{1.0f};
     float _cos_pitch{1.0f};

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -1057,6 +1057,28 @@ bool AP_AHRS_DCM::get_location(Location &loc) const
     return _have_position;
 }
 
+bool AP_AHRS_DCM::set_location(const Location &loc) {
+    // Z coordinate is not processed in the functions below
+    if (!ready_to_accept_location()) {
+        return false;
+    }
+
+    _last_lat = loc.lat;
+    _last_lng = loc.lng;
+    _last_pos_ms = AP_HAL::millis();
+    _position_offset_north = 0;
+    _position_offset_east = 0;
+
+    // once we have a single GPS lock, we can update using
+    // dead-reckoning from then on
+    _have_position = true;
+    return true;
+}
+
+bool AP_AHRS_DCM::ready_to_accept_location() const {
+    return !have_gps();
+}
+
 bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
 {
 #if AP_AIRSPEED_ENABLED

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -131,6 +131,11 @@ class AP_AHRS_DCM : public AP_AHRS_Backend {
 
     void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
 
+    bool set_location(const Location &loc);
+
+    // returns true if DCM is ready to accept location, false otherwise
+    bool ready_to_accept_location() const;
+
 private:
 
     // dead-reckoning support

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -1080,6 +1080,36 @@ bool NavEKF2::getLLH(Location &loc) const
     return core[primary].getLLH(loc);
 }
 
+// Sets the location if GPS is not available
+bool NavEKF2::setLLH(const Location &location) {
+    // Z coordinate is not processed in the functions below
+    if (!core) {
+        return false;
+    }
+    uint8_t succeed_count = 0;
+    for (uint8_t i=0; i<num_cores; i++) {
+         if (core[i].setLLH(location)) {
+             succeed_count++;
+         }
+    }
+    return succeed_count == num_cores;
+}
+
+// Checks if ready to accept new location
+bool NavEKF2::readyToAcceptLLH() const
+{
+    if (!core) {
+        return false;
+    }
+    uint8_t succeed_count = 0;
+    for (uint8_t i=0; i<num_cores; i++) {
+         if (core[i].readyToAcceptLLH()) {
+             succeed_count++;
+         }
+    }
+    return succeed_count == num_cores;
+}
+
 // Return the latitude and longitude and height used to set the NED origin for the specified instance
 // An out of range instance (eg -1) returns data for the primary instance
 // All NED positions calculated by the filter are relative to this location

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -131,6 +131,15 @@ class NavEKF2 {
     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     bool getLLH(Location &loc) const;
 
+    // Sets the location if GPS is not available
+    // the location is not checked for validity. Should be checked by a caller invalid
+    // returns true if succeeded, false if GPS measurements are available
+    bool setLLH(const Location &location);
+
+    // Checks if ready to accept new location
+    // Returns true if GPS is not available and origin is valid for all cores, false otherwise
+    bool readyToAcceptLLH() const;
+
     // Return the latitude and longitude and height used to set the NED origin for the specified instance
     // An out of range instance (eg -1) returns data for the primary instance
     // All NED positions calculated by the filter are relative to this location

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -877,6 +877,25 @@ void NavEKF2_core::calcOutputStates()
     }
 }
 
+bool NavEKF2_core::setLLH(const Location &location) {
+    // Z coordinate is not processed, to be implemented
+    if (gpsNotAvailable) {
+        Location origin;
+        if (getOriginLLH(origin)) {
+            auto ned = origin.get_distance_NED(location);
+            ResetPositionNE(ned.x, ned.y);
+            return true;
+        }
+        return false; // Origin not set, so we can not calculate offset from it 
+    }
+    return false;
+}
+
+bool NavEKF2_core::readyToAcceptLLH() const
+{
+    return gpsNotAvailable && validOrigin;
+}
+
 /*
  * Calculate the predicted state covariance matrix using algebraic equations generated with Matlab symbolic toolbox.
  * The script file used to generate these and other equations in this filter can be found here:

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -166,6 +166,14 @@ class NavEKF2_core : public NavEKF_core_common
     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     bool getLLH(Location &loc) const;
 
+    // Sets the location if GPS is not available
+    // returns true if succeeded, false if GPS measurements are available or location is invalid
+    bool setLLH(const Location &location);
+
+    // Checks if ready to accept new location
+    // Returns true if GPS is not available and origin is valid for all cores, false otherwise
+    bool readyToAcceptLLH() const;
+
     // return the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter are relative to this location
     // Returns false if the origin has not been set

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1366,6 +1366,35 @@ bool NavEKF3::getLLH(Location &loc) const
     return core[primary].getLLH(loc);
 }
 
+bool NavEKF3::setLLH(const Location &location) {
+    // Z coordinate is not processed in the functions below
+    if (!core) {
+        return false;
+    }
+    uint8_t succeed_count = 0;
+    for (uint8_t i=0; i<num_cores; i++) {
+        if (core[i].setLLH(location)) {
+            succeed_count++;
+        }
+    }
+    return succeed_count == num_cores;
+}
+
+// Checks if ready to accept new location
+bool NavEKF3::readyToAcceptLLH() const
+{
+    if (!core) {
+        return false;
+    }
+    uint8_t succeed_count = 0;
+    for (uint8_t i=0; i<num_cores; i++) {
+        if (core[i].readyToAcceptLLH()) {
+            succeed_count++;
+        }
+    }
+    return succeed_count == num_cores;
+}
+
 // Return the latitude and longitude and height used to set the NED origin
 // All NED positions calculated by the filter are relative to this location
 // Returns false if the origin has not been set

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -141,6 +141,15 @@ class NavEKF3 {
     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     bool getLLH(Location &loc) const;
 
+    // Sets the location if GPS is not available
+    // the location is not checked for validity. Should be checked by a caller invalid
+    // returns true if succeeded, false if GPS measurements are available
+    bool setLLH(const Location &location);
+
+    // Checks if ready to accept new location
+    // Returns true if GPS is not available and origin is valid for all cores, false otherwise
+    bool readyToAcceptLLH() const;
+
     // Return the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter are relative to this location
     // Returns false if the origin has not been set

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -1020,6 +1020,25 @@ void NavEKF3_core::calcOutputStates()
     }
 }
 
+bool NavEKF3_core::setLLH(const Location &location) {
+    // Z coordinate is not processed, to be implemented
+    if (!gpsIsInUse) {
+        if (validOrigin) {
+            Location origin{EKF_origin};
+            auto ned = origin.get_distance_NED(location);
+            ResetPositionNE(ned.x, ned.y);
+            return true;
+        }
+        return false; // Origin not set, so we can not calculate offset from it 
+    }
+    return false;
+}
+
+bool NavEKF3_core::readyToAcceptLLH() const
+{
+    return !gpsIsInUse && validOrigin;
+}
+
 /*
  * Calculate the predicted state covariance matrix using algebraic equations generated using SymPy
  * See AP_NavEKF3/derivation/main.py for derivation

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -220,6 +220,15 @@ class NavEKF3_core : public NavEKF_core_common
     // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
     bool getLLH(Location &loc) const;
 
+    // Sets the location if GPS is not available
+    // the location is not checked for validity. Should be checked by a caller invalid
+    // returns true if succeeded, false if GPS measurements are available
+    bool setLLH(const Location &location);
+
+    // Checks if ready to accept new location
+    // Returns true if GPS is not available and origin is valid for all cores, false otherwise
+    bool readyToAcceptLLH() const;
+
     // return the latitude and longitude and height used to set the NED origin
     // All NED positions calculated by the filter are relative to this location
     // Returns false if the origin has not been set

libraries/AP_Scripting/docs/docs.lua

@@ -3172,6 +3172,11 @@ function ahrs:get_home() end
 ---@return Location_ud|nil
 function ahrs:get_location() end
 
+-- desc
+---@param value Location_ud
+---@return boolean
+function ahrs:set_location(value) end
+
 -- same as `get_location` will be removed
 ---@return Location_ud|nil
 function ahrs:get_position() end

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

@@ -0,0 +1,72 @@
+-- Example of receiving MAVLink commands
+
+local mavlink_msgs = require("MAVLink/mavlink_msgs")
+
+local COMMAND_ACK_ID = mavlink_msgs.get_msgid("COMMAND_ACK")
+local COMMAND_LONG_ID = mavlink_msgs.get_msgid("COMMAND_LONG")
+
+local msg_map = {}
+msg_map[COMMAND_ACK_ID] = "COMMAND_ACK"
+msg_map[COMMAND_LONG_ID] = "COMMAND_LONG"
+
+-- initialize MAVLink rx with number of messages, and buffer depth
+mavlink:init(1, 10)
+
+-- register message id to receive
+mavlink:register_rx_msgid(COMMAND_LONG_ID)
+
+local MAV_CMD_USER_1 = 31010
+local MAV_CMD_DO_YOU_ARE_HERE = MAV_CMD_USER_1
+
+function handle_command_long(cmd)
+    if (cmd.command == MAV_CMD_DO_YOU_ARE_HERE) then
+        if cmd.param5 == 0 or cmd.param6 == 0 {
+            return 2 -- MAV_RESULT_DENIED
+        }
+        local new_ahrs_location
+        if cmd.param7 == 0 {
+            new_ahrs_location = ahrs:get_location()
+        } else {
+            new_ahrs_location = Location()
+            new_ahrs_location:alt(math.floor(cmd.param7*100))
+        }
+        if new_ahrs_location ~= nil then
+            new_ahrs_location:lat(math.floor(cmd.param5*1.0e7))
+            new_ahrs_location:lng(math.floor(cmd.param6*1.0e7))
+            return 0 -- MAV_RESULT_ACCEPTED
+        end
+        return 4 -- MAV_RESULT_FAILED
+    end
+    return nil
+end
+
+function update()
+    local msg, chan = mavlink:receive_chan()
+    if (msg ~= nil) then
+        local parsed_msg = mavlink_msgs.decode(msg, msg_map)
+        if (parsed_msg ~= nil) then
+
+            local result
+            if parsed_msg.msgid == COMMAND_LONG_ID then
+                result = handle_command_long(parsed_msg)
+            end
+
+            if (result ~= nil) then
+                -- Send ack if the command is one were intrested in
+                local ack = {}
+                ack.command = parsed_msg.command
+                ack.result = result
+                ack.progress = 0
+                ack.result_param2 = 0
+                ack.target_system = parsed_msg.sysid
+                ack.target_component = parsed_msg.compid
+
+                mavlink:send_chan(chan, mavlink_msgs.encode("COMMAND_ACK", ack))
+            end
+        end
+    end
+
+    return update, 1000
+end
+
+return update()

libraries/AP_Scripting/generator/description/bindings.desc

@@ -33,6 +33,7 @@ singleton AP_AHRS method get_pitch float
 singleton AP_AHRS method get_yaw float
 singleton AP_AHRS method get_location boolean Location'Null
 singleton AP_AHRS method get_location alias get_position
+singleton AP_AHRS method set_location boolean Location
 singleton AP_AHRS method get_home Location
 singleton AP_AHRS method get_gyro Vector3f
 singleton AP_AHRS method get_accel Vector3f