From 27609ebb055f8afdcae03209a19e91bf6753600a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 May 2023 11:53:57 +1000 Subject: [PATCH 1/8] mavlink: update for external position commmand --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index e7297c9f15ecb..8e44a3f3a73b9 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit e7297c9f15ecb631fa4d7e0ead086be8d918fac3 +Subproject commit 8e44a3f3a73b982212895886d7d85cff9a1483d8 From 1f8bf4f2865873b624400300976de85c31089423 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 May 2023 09:55:02 +1000 Subject: [PATCH 2/8] AP_DAL: Add handlers for external lat lng position set --- libraries/AP_DAL/AP_DAL.cpp | 22 ++++++++++++++++++++++ libraries/AP_DAL/AP_DAL.h | 4 ++++ libraries/AP_DAL/LogStructure.h | 13 +++++++++++++ 3 files changed, 39 insertions(+) diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 8e2b6fe87b7da..8942a6e46d170 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -352,6 +352,17 @@ void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old); } +void AP_DAL::log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms) +{ + end_frame(); + const log_RSLL old = _RSLL; + _RSLL.lat = loc.lat; + _RSLL.lng = loc.lng; + _RSLL.posAccSD = posAccuracy; + _RSLL.timestamp_ms = timestamp_ms; + WRITE_REPLAY_BLOCK_IFCHANGED(RSLL, _RSLL, old); +} + // log external velocity data void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { @@ -485,6 +496,17 @@ void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) // note that EKF2 does not support body frame odomotry ekf3.writeBodyFrameOdom(msg.quality, msg.delPos, msg.delAng, msg.delTime, msg.timeStamp_ms, msg.delay_ms, msg.posOffset); } + +/* + handle position reset + */ +void AP_DAL::handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) +{ + _RSLL = msg; + // note that EKF2 does not support body frame odomotry + const Location loc {msg.lat, msg.lng, 0, Location::AltFrame::ABSOLUTE }; + ekf3.setLatLng(loc, msg.posAccSD, msg.timestamp_ms); +} #endif // APM_BUILD_Replay namespace AP { diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 8b86a4122d360..ddf643bcf6e33 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/libraries/AP_DAL/AP_DAL.h @@ -82,6 +82,8 @@ class AP_DAL { void log_event3(Event event); void log_SetOriginLLH3(const Location &loc); + void log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms); + void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty); void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type); @@ -317,6 +319,7 @@ class AP_DAL { void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3); void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3); void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3); + void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3); // map core number for replay uint8_t logging_core(uint8_t c) const; @@ -340,6 +343,7 @@ class AP_DAL { struct log_REVH _REVH; struct log_RWOH _RWOH; struct log_RBOH _RBOH; + struct log_RSLL _RSLL; // cached variables for speed: uint32_t _micros; diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index e391a6dcf4469..14942768f35c1 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -35,6 +35,7 @@ LOG_RMGI_MSG, \ LOG_ROFH_MSG, \ LOG_REPH_MSG, \ + LOG_RSLL_MSG, \ LOG_REVH_MSG, \ LOG_RWOH_MSG, \ LOG_RBOH_MSG @@ -324,6 +325,16 @@ struct log_REPH { uint8_t _end; }; +// @LoggerMessage: RSLL +// @Description: Replay Set Lat Lng event +struct log_RSLL { + int32_t lat; // WGS-84 latitude in 1E-7 degrees + int32_t lng; // WGS-84 longitude in 1E7 degrees + float posAccSD; // horizontal position 1 STD uncertainty (m) + uint32_t timestamp_ms; + uint8_t _end; +}; + // @LoggerMessage: REVH // @Description: Replay external position data struct log_REVH { @@ -417,6 +428,8 @@ struct log_RBOH { "ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \ { LOG_REPH_MSG, RLOG_SIZE(REPH), \ "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \ + { LOG_RSLL_MSG, RLOG_SIZE(RSLL), \ + "RSLL", "IIfI", "Lat,Lng,PosAccSD,TS", "DU--", "GG--" }, \ { LOG_REVH_MSG, RLOG_SIZE(REVH), \ "REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \ { LOG_RWOH_MSG, RLOG_SIZE(RWOH), \ From 86eeb7d3532c0a0ee756a89b17eb0523a6a5a286 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 May 2023 09:55:30 +1000 Subject: [PATCH 3/8] AP_NavEKF3: Add handlers for external lat lng position set --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 20 +++++++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 5 +++ .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 36 +++++++++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 5 +++ libraries/AP_NavEKF3/AP_NavEKF3_feature.h | 5 +++ 5 files changed, 71 insertions(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 45e8b1449e03e..e3af24cf7d2a2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1408,6 +1408,26 @@ bool NavEKF3::setOriginLLH(const Location &loc) return ret; } +bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms) +{ +#if EK3_FEATURE_POSITION_RESET + AP::dal().log_SetLatLng(loc, posAccuracy, timestamp_ms); + + if (!core) { + return false; + } + bool ret = false; + for (uint8_t i=0; ideadReckonDeclare_ms || + (PV_AidingMode == AID_NONE) + || !validOrigin) { + return false; + } + + // Store the position before the reset so that we can record the reset delta + posResetNE.x = stateStruct.position.x; + posResetNE.y = stateStruct.position.y; + + // reset the corresponding covariances + zeroRows(P,7,8); + zeroCols(P,7,8); + + // set the variances using the position measurement noise parameter + P[7][7] = P[8][8] = sq(MAX(posAccuracy,frontend->_gpsHorizPosNoise)); + + // Correct the position for time delay relative to fusion time horizon assuming a constant velocity + // Limit time stamp to a range between current time and 5 seconds ago + const uint32_t timeStampConstrained_ms = MAX(MIN(timestamp_ms, imuSampleTime_ms), imuSampleTime_ms - 5000); + const int32_t delta_ms = int32_t(imuDataDelayed.time_ms - timeStampConstrained_ms); + const ftype delaySec = 1E-3F * ftype(delta_ms); + const Vector2F newPosNE = EKF_origin.get_distance_NE_ftype(loc) - stateStruct.velocity.xy() * delaySec; + ResetPositionNE(newPosNE.x,newPosNE.y); + + return true; +} +#endif // EK3_FEATURE_POSITION_RESET + + // reset the stateStruct's NE position to the specified position // posResetNE is updated to hold the change in position // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 0bb9739b23dbe..8ba964b7e88fd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -230,6 +230,11 @@ class NavEKF3_core : public NavEKF_core_common // returns false if Absolute aiding and GPS is being used or if the origin is already set bool setOriginLLH(const Location &loc); + // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty + // The altitude element of the location is not used. + // Returns true if the set was successful + bool setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms); + // return estimated height above ground level // return false if ground height is not being estimated. bool getHAGL(float &HAGL) const; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h index 620540b994e6c..3cd2bc3866d0d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h @@ -7,6 +7,7 @@ #include #include #include +#include // define for when to include all features #define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay) @@ -30,3 +31,7 @@ #ifndef EK3_FEATURE_BEACON_FUSION #define EK3_FEATURE_BEACON_FUSION AP_BEACON_ENABLED #endif + +#ifndef EK3_FEATURE_POSITION_RESET +#define EK3_FEATURE_POSITION_RESET EK3_FEATURE_ALL || AP_AHRS_POSITION_RESET_ENABLED +#endif From 26d800cd63c6a770e6a004c6488c444648d39103 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 May 2023 09:55:43 +1000 Subject: [PATCH 4/8] Tools/Replay: Add handlers for external lat lng position set --- Tools/Replay/LR_MsgHandler.cpp | 6 ++++++ Tools/Replay/LR_MsgHandler.h | 6 ++++++ Tools/Replay/LogReader.cpp | 2 ++ 3 files changed, 14 insertions(+) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 8b9d2bd431511..cbdca233afaf6 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -289,6 +289,12 @@ void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes) AP::dal().handle_message(msg, ekf2, ekf3); } +void LR_MsgHandler_RSLL::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(RSLL, msgbytes); + AP::dal().handle_message(msg, ekf2, ekf3); +} + void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes) { MSG_CREATE(REVH, msgbytes); diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index b258f496e728a..4e30a6571d35c 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -57,6 +57,12 @@ class LR_MsgHandler_REPH : public LR_MsgHandler_EKF void process_message(uint8_t *msg) override; }; +class LR_MsgHandler_RSLL : public LR_MsgHandler_EKF +{ + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; + void process_message(uint8_t *msg) override; +}; + class LR_MsgHandler_REVH : public LR_MsgHandler_EKF { using LR_MsgHandler_EKF::LR_MsgHandler_EKF; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index d36c3da3fe9e1..9a84053ab81ad 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -118,6 +118,8 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = new LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3); } else if (streq(name, "REPH")) { msgparser[f.type] = new LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3); + } else if (streq(name, "RSLL")) { + msgparser[f.type] = new LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3); } else if (streq(name, "REVH")) { msgparser[f.type] = new LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3); } else if (streq(name, "RWOH")) { From bc878e14afc21e871c25e755737b0bfccbfc069d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 May 2023 09:56:34 +1000 Subject: [PATCH 5/8] Tools/autotest: Add external lat lng position set to replay message list --- Tools/autotest/common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index b30c77ee95b0b..abbfaf9747873 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2977,7 +2977,7 @@ def LoggerDocumentation(self): REPLAY_MSGS = ['RFRH', 'RFRF', 'REV2', 'RSO2', 'RWA2', 'REV3', 'RSO3', 'RWA3', 'RMGI', 'REY3', 'RFRN', 'RISH', 'RISI', 'RISJ', 'RBRH', 'RBRI', 'RRNH', 'RRNI', 'RGPH', 'RGPI', 'RGPJ', 'RASH', 'RASI', 'RBCH', 'RBCI', 'RVOH', 'RMGH', - 'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH'] + 'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH', 'RSLL'] docco_ids = {} for thing in tree.logformat: From 26cbc6b389781325adda4dcd80fce4e19a50027a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 May 2023 09:56:58 +1000 Subject: [PATCH 6/8] AP_AHRS: Add handlers for external lat lng position set --- libraries/AP_AHRS/AP_AHRS.cpp | 10 ++++++++++ libraries/AP_AHRS/AP_AHRS.h | 11 +++++++++++ libraries/AP_AHRS/AP_AHRS_config.h | 5 +++++ 3 files changed, 26 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4cd2eda3a24eb..86f5bb4f9ec4b 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1287,6 +1287,16 @@ bool AP_AHRS::set_origin(const Location &loc) return false; } +#if AP_AHRS_POSITION_RESET_ENABLED +bool AP_AHRS::handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_ms) +{ +#if HAL_NAVEKF3_AVAILABLE + return EKF3.setLatLng(loc, pos_accuracy, timestamp_ms); +#endif + return false; +} +#endif + // return true if inertial navigation is active bool AP_AHRS::have_inertial_nav(void) const { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 6c11a0a55ccd8..f3cc56f057d94 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -194,6 +194,17 @@ class AP_AHRS { // from which to decide the origin on its own bool set_origin(const Location &loc) WARN_IF_UNUSED; +#if AP_AHRS_POSITION_RESET_ENABLED + // Set the EKF's NE horizontal position states and their corresponding variances from the supplied WGS-84 location + // and 1-sigma horizontal position uncertainty. This can be used when the EKF is dead reckoning to periodically + // correct the position. If the EKF is is still using data from a postion sensor such as GPS, the position set + // will not be performed. + // pos_accuracy is the standard deviation of the horizontal position uncertainty in metres. + // The altitude element of the location is not used. + // Returns true if the set was successful. + bool handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_); +#endif + // returns the inertial navigation origin in lat/lon/alt bool get_origin(Location &ret) const WARN_IF_UNUSED; diff --git a/libraries/AP_AHRS/AP_AHRS_config.h b/libraries/AP_AHRS/AP_AHRS_config.h index e600487d1d61f..5fbc42337af35 100644 --- a/libraries/AP_AHRS/AP_AHRS_config.h +++ b/libraries/AP_AHRS/AP_AHRS_config.h @@ -14,3 +14,8 @@ #ifndef AP_AHRS_SIM_ENABLED #define AP_AHRS_SIM_ENABLED AP_SIM_ENABLED #endif + +#ifndef AP_AHRS_POSITION_RESET_ENABLED +#define AP_AHRS_POSITION_RESET_ENABLED (BOARD_FLASH_SIZE>1024) +#endif + From 2f9af53a352c07fc883492954c147f779fb1021a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 27 May 2023 19:46:06 +1000 Subject: [PATCH 7/8] GCS_MAVLink: support EXTERNAL_POSITION_ESTIMATE command_int --- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 37 +++++++++++++++++++++++++++- 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index d0134a7d6ae0c..0168e0de6f1fb 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -516,6 +516,7 @@ class GCS_MAVLINK MAV_RESULT handle_command_int_do_set_home(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_int_external_position_estimate(const mavlink_command_int_t &packet); virtual bool set_home_to_current_location(bool lock) = 0; virtual bool set_home(const Location& loc, bool lock) = 0; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 337700a966a72..bd1d5e3d0fbc5 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4816,7 +4816,7 @@ bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Locat } // integer storage imposes limits on the altitudes we can accept: - if (fabsf(in.z) > LOCATION_ALT_MAX_M) { + if (isnan(in.z) || fabsf(in.z) > LOCATION_ALT_MAX_M) { return false; } @@ -4842,6 +4842,7 @@ bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command) case MAV_CMD_DO_SET_ROI_LOCATION: case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_REPOSITION: + case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: return true; default: return false; @@ -4980,6 +4981,36 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_do_set_home(const mavlink_command_int return MAV_RESULT_ACCEPTED; } +#if AP_AHRS_POSITION_RESET_ENABLED +MAV_RESULT GCS_MAVLINK::handle_command_int_external_position_estimate(const mavlink_command_int_t &packet) +{ + if (packet.frame != MAV_FRAME_GLOBAL || + !isnan(packet.z)) { + // we only support global frame without altitude + return MAV_RESULT_DENIED; + } + + // cope with the NaN when convering to Location + Location loc; + mavlink_command_int_t p2 = packet; + p2.z = 0; + + if (!location_from_command_t(p2, loc)) { + return MAV_RESULT_DENIED; + } + uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(uint64_t(p2.param1*1e6), PAYLOAD_SIZE(chan, COMMAND_INT)); + const uint32_t processing_ms = p2.param2*1e3; + const float pos_accuracy = p2.param3; + if (timestamp_ms > processing_ms) { + timestamp_ms -= processing_ms; + } + if (!AP::ahrs().handle_external_position_estimate(loc, pos_accuracy, timestamp_ms)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; +} +#endif // AP_AHRS_POSITION_RESET_ENABLED + MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi_none() { #if HAL_MOUNT_ENABLED @@ -5081,6 +5112,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_do_set_roi_sysid(packet); case MAV_CMD_DO_SET_HOME: return handle_command_int_do_set_home(packet); +#if AP_AHRS_POSITION_RESET_ENABLED + case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: + return handle_command_int_external_position_estimate(packet); +#endif #if AP_SCRIPTING_ENABLED case MAV_CMD_SCRIPTING: From ea194cea723eed8a4258fb8b7a18751128e7c812 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Jun 2023 07:16:25 +1000 Subject: [PATCH 8/8] Tools: added test for MAV_CMD_EXTERNAL_POSITION_ESTIMATE --- Tools/autotest/arduplane.py | 90 +++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index cbf317a94ec9c..b68e25c10310f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -579,6 +579,95 @@ def DO_REPOSITION(self): self.fly_home_land_and_disarm() + def ExternalPositionEstimate(self): + '''Test mavlink EXTERNAL_POSITION_ESTIMATE command''' + if not hasattr(mavutil.mavlink, 'MAV_CMD_EXTERNAL_POSITION_ESTIMATE'): + return + self.change_mode("TAKEOFF") + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_altitude(48, 52, relative=True) + + loc = self.mav.location() + self.location_offset_ne(loc, 2000, 2000) + + # setting external position fail while we have GPS lock + self.progress("set new position with GPS") + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_EXTERNAL_POSITION_ESTIMATE, + self.get_sim_time()-1, # transmit time + 0.5, # processing delay + 50, # accuracy + 0, + int(loc.lat * 1e7), + int(loc.lng * 1e7), + float("NaN"), # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + want_result=mavutil.mavlink.MAV_RESULT_FAILED, + ) + + self.progress("disable the GPS") + self.run_auxfunc( + 65, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + # fly for a bit to get into non-aiding state + self.progress("waiting 20 seconds") + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + 20: + self.wait_heartbeat() + + self.progress("getting base position") + gpi = self.mav.recv_match( + type='GLOBAL_POSITION_INT', + blocking=True, + timeout=5 + ) + loc = mavutil.location(gpi.lat*1e-7, gpi.lon*1e-7, 0, 0) + + self.progress("set new position with no GPS") + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_EXTERNAL_POSITION_ESTIMATE, + self.get_sim_time()-1, # transmit time + 0.5, # processing delay + 50, # accuracy + 0, + gpi.lat+1, + gpi.lon+1, + float("NaN"), # alt + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + self.progress("waiting 3 seconds") + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + 3: + self.wait_heartbeat() + + gpi2 = self.mav.recv_match( + type='GLOBAL_POSITION_INT', + blocking=True, + timeout=5 + ) + loc2 = mavutil.location(gpi2.lat*1e-7, gpi2.lon*1e-7, 0, 0) + dist = self.get_distance(loc, loc2) + + self.progress("dist is %.1f" % dist) + if dist > 200: + raise NotAchievedException("Position error dist=%.1f" % dist) + + self.progress("re-enable the GPS") + self.run_auxfunc( + 65, + 0, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + self.progress("flying home") + self.fly_home_land_and_disarm() + def DeepStall(self): '''Test DeepStall Landing''' # self.fly_deepstall_absolute() @@ -4603,6 +4692,7 @@ def tests(self): self.SDCardWPTest, self.NoArmWithoutMissionItems, self.MODE_SWITCH_RESET, + self.ExternalPositionEstimate, ]) return ret