From 0bea8abddeb4ba91d74ecbe32794d3e0b53b0a38 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Oct 2024 11:06:24 +1000 Subject: [PATCH 01/16] AP_AHRS: DCM: log estimated wind --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index fd0b8677561eb..d64dc1b628784 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -116,18 +116,24 @@ AP_AHRS_DCM::update() // @Field: Yaw: estimated yaw // @Field: ErrRP: lowest estimated gyro drift error // @Field: ErrYaw: difference between measured yaw and DCM yaw estimate +// @Field: VWN: wind velocity, to-the-North component +// @Field: VWE: wind velocity, to-the-East component +// @Field: VWD: wind velocity, Up-to-Down component AP::logger().WriteStreaming( "DCM", - "TimeUS," "Roll," "Pitch," "Yaw," "ErrRP," "ErrYaw", - "s" "d" "d" "d" "d" "h", - "F" "0" "0" "0" "0" "0", - "Q" "f" "f" "f" "f" "f", + "TimeUS," "Roll," "Pitch," "Yaw," "ErrRP," "ErrYaw," "VWN," "VWE," "VWD", + "s" "d" "d" "d" "d" "h" "n" "n" "n", + "F" "0" "0" "0" "0" "0" "0" "0" "0", + "Q" "f" "f" "f" "f" "f" "f" "f" "f", AP_HAL::micros64(), degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), get_error_rp(), - get_error_yaw() + get_error_yaw(), + _wind.x, + _wind.y, + _wind.z ); } #endif // HAL_LOGGING_ENABLED From 79e11242d2a056205efaf6c393807cec073aca75 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Oct 2024 11:06:54 +1000 Subject: [PATCH 02/16] AP_NavEKF3: clarify wind direction descriptions --- libraries/AP_NavEKF3/LogStructure.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 9218c6d60bf84..53276e13d3c53 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -104,8 +104,8 @@ struct PACKED log_XKF1 { // @Field: AX: Estimated accelerometer X bias // @Field: AY: Estimated accelerometer Y bias // @Field: AZ: Estimated accelerometer Z bias -// @Field: VWN: Estimated wind velocity (North component) -// @Field: VWE: Estimated wind velocity (East component) +// @Field: VWN: Estimated wind velocity (moving-to-North component) +// @Field: VWE: Estimated wind velocity (moving-to-East component) // @Field: MN: Magnetic field strength (North component) // @Field: ME: Magnetic field strength (East component) // @Field: MD: Magnetic field strength (Down component) From 89064642c307751c6494b7d09e7e9988d3e590ce Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Oct 2024 11:07:25 +1000 Subject: [PATCH 03/16] SITL: clarify wind direction descriptions --- libraries/SITL/SITL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 255b44ff4d3b9..970fdd6a52f41 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -86,7 +86,7 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @User: Advanced AP_GROUPINFO("WIND_SPD", 9, SIM, wind_speed, 0), // @Param: WIND_DIR - // @DisplayName: Simulated Wind direction + // @DisplayName: Direction simulated wind is coming from // @Description: Allows you to set wind direction (true deg) in sim // @Units: deg // @User: Advanced From 57dd342c061ebc526af8b98c084810e4c87346b0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 8 Oct 2024 09:06:34 +1100 Subject: [PATCH 04/16] AP_NavEKF2: clarify wind direction descriptions --- libraries/AP_NavEKF2/LogStructure.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/LogStructure.h b/libraries/AP_NavEKF2/LogStructure.h index a7f1a8862557e..8d61f208c7a09 100644 --- a/libraries/AP_NavEKF2/LogStructure.h +++ b/libraries/AP_NavEKF2/LogStructure.h @@ -100,8 +100,8 @@ struct PACKED log_NKF1 { // @Field: GSX: Gyro Scale Factor (X-axis) // @Field: GSY: Gyro Scale Factor (Y-axis) // @Field: GSZ: Gyro Scale Factor (Z-axis) -// @Field: VWN: Estimated wind velocity (North component) -// @Field: VWE: Estimated wind velocity (East component) +// @Field: VWN: Estimated wind velocity (moving-to-North component) +// @Field: VWE: Estimated wind velocity (moving-to-East component) // @Field: MN: Magnetic field strength (North component) // @Field: ME: Magnetic field strength (East component) // @Field: MD: Magnetic field strength (Down component) From c6c22837121b8fbbd20b720a22863e280cae50ac Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 Sep 2024 13:27:20 +1000 Subject: [PATCH 05/16] AP_DAL: Handle external wind state set --- libraries/AP_DAL/AP_DAL.cpp | 22 ++++++++++++++++++++++ libraries/AP_DAL/AP_DAL.h | 3 +++ libraries/AP_DAL/LogStructure.h | 17 +++++++++++++++-- 3 files changed, 40 insertions(+), 2 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 4d6a1f9bd6ad6..9e62e9455e6d3 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -381,6 +381,17 @@ void AP_DAL::log_SetLatLng(const Location &loc, float posAccuracy, uint32_t time WRITE_REPLAY_BLOCK_IFCHANGED(RSLL, _RSLL, old); } +void AP_DAL::log_SetWind(float speed, float speed_accuracy, float direction, float direction_accuracy) +{ + end_frame(); + const log_RSWS old = _RSWS; + _RSWS.speed = speed; + _RSWS.speed_accuracy = speed_accuracy; + _RSWS.direction = direction; + _RSWS.direction_accuracy = direction_accuracy; + WRITE_REPLAY_BLOCK_IFCHANGED(RSWS, _RSWS, old); +} + // log external velocity data void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { @@ -527,6 +538,17 @@ void AP_DAL::handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) const Location loc {msg.lat, msg.lng, 0, Location::AltFrame::ABSOLUTE }; ekf3.setLatLng(loc, msg.posAccSD, msg.timestamp_ms); } +/* + handle wind speed and direction set + */ +void AP_DAL::handle_message(const log_RSWS &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) +{ + _RSWS = msg; + // note that EKF2 does not support setting wind speed +#if EK3_FEATURE_SETWIND + ekf3.setWind(msg.speed, msg.speed_accuracy, msg.direction, msg.direction_accuracy); +#endif +} #endif // APM_BUILD_Replay namespace AP { diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 64a16fff45608..c4d8ac2058274 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/libraries/AP_DAL/AP_DAL.h @@ -83,6 +83,7 @@ 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_SetWind(float speed, float speed_accuracy, float direction, float direction_accuracy); void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty); void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type); @@ -328,6 +329,7 @@ class AP_DAL { 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); + void handle_message(const log_RSWS &msg, NavEKF2 &ekf2, NavEKF3 &ekf3); // map core number for replay uint8_t logging_core(uint8_t c) const; @@ -354,6 +356,7 @@ class AP_DAL { struct log_RWOH _RWOH; struct log_RBOH _RBOH; struct log_RSLL _RSLL; + struct log_RSWS _RSWS; // cached variables for speed: uint32_t _micros; diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 873251c7dd63e..cd44dd6dee22c 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -38,7 +38,8 @@ LOG_RSLL_MSG, \ LOG_REVH_MSG, \ LOG_RWOH_MSG, \ - LOG_RBOH_MSG + LOG_RBOH_MSG, \ + LOG_RSWS_MSG // Replay Data Structures struct log_RFRH { @@ -335,6 +336,16 @@ struct log_RSLL { uint8_t _end; }; +// @LoggerMessage: RSWS +// @Description: Replay set wind speed event +struct log_RSWS { + float speed; // wind speed (m/s) + float speed_accuracy; // wind speed 1-sigma uncertainty (m/s) + float direction; // wind direction (deg) + float direction_accuracy; // wind direction 1-signma uncertainty (deg) + uint8_t _end; +}; + // @LoggerMessage: REVH // @Description: Replay external position data struct log_REVH { @@ -435,4 +446,6 @@ struct log_RBOH { { LOG_RWOH_MSG, RLOG_SIZE(RWOH), \ "RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" }, \ { LOG_RBOH_MSG, RLOG_SIZE(RBOH), \ - "RBOH", "ffffffffIfffH", "Q,DPX,DPY,DPZ,DAX,DAY,DAZ,DT,TS,OX,OY,OZ,D", "-------------", "-------------" }, + "RBOH", "ffffffffIfffH", "Q,DPX,DPY,DPZ,DAX,DAY,DAZ,DT,TS,OX,OY,OZ,D", "-------------", "-------------" }, \ + { LOG_RSWS_MSG, RLOG_SIZE(RSWS), \ + "RSWS", "ffff", "Spd,SpdErr,Dirn,DirnErr", "----", "----" }, From 993e5f89c4723cc3abca6a8887573a556a6b58a9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 28 Aug 2024 08:47:38 +1000 Subject: [PATCH 06/16] Replay: Handle external wind state set --- Tools/Replay/LR_MsgHandler.cpp | 6 ++++++ Tools/Replay/LR_MsgHandler.h | 6 ++++++ Tools/Replay/LogReader.cpp | 4 +++- 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index cbdca233afaf6..55014420b25c0 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -295,6 +295,12 @@ void LR_MsgHandler_RSLL::process_message(uint8_t *msgbytes) AP::dal().handle_message(msg, ekf2, ekf3); } +void LR_MsgHandler_RSWS::process_message(uint8_t *msgbytes) +{ + MSG_CREATE(RSWS, 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 4e30a6571d35c..8d8a67a86518b 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -63,6 +63,12 @@ class LR_MsgHandler_RSLL : public LR_MsgHandler_EKF void process_message(uint8_t *msg) override; }; +class LR_MsgHandler_RSWS : 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 0691cff6b4c19..f06fb9e19e0d1 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -126,7 +126,9 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3); } else if (streq(name, "RBOH")) { msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3); - } else { + } else if (streq(name, "RSWS")) { + msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSWS(formats[f.type], ekf2, ekf3); + } else { // debug(" No parser for (%s)\n", name); } From e9f5399dbbda923f5c39217e07bfbe203750de01 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 Sep 2024 12:50:10 +1000 Subject: [PATCH 07/16] AP_NavEKF3: Allow wind state to be set externally --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 17 ++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 8 +++ .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 57 +++++++++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 32 +++++++++-- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 4 ++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 16 ++++++ libraries/AP_NavEKF3/AP_NavEKF3_feature.h | 4 ++ libraries/AP_NavEKF3/derivation/generate_1.py | 24 ++++++++ .../polar_wind_to_cartesian_covariance.cpp | 14 +++++ 9 files changed, 170 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_NavEKF3/derivation/generated/polar_wind_to_cartesian_covariance.cpp diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 7e8dfadb0e5c5..e3bc920603711 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1435,6 +1435,23 @@ bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timesta #endif // EK3_FEATURE_POSITION_RESET } +#if EK3_FEATURE_SETWIND +bool NavEKF3::setWind(float speed, float speed_accuracy, float direction, float direction_accuracy) +{ + if (!core) { + return false; + } + + AP::dal().log_SetWind(speed, speed_accuracy, direction, direction_accuracy); + + bool ret = false; + for (uint8_t i=0; i frontend->deadReckonDeclare_ms) { + velResetNE = stateStruct.wind_vel - wind_vel_prev; + stateStruct.velocity.xy() += velResetNE; + + // update the output buffer + for (uint8_t i=0; i sq(5.0f) || dragFusionEnabled)) { inhibitWindStates = false; @@ -81,9 +84,15 @@ void NavEKF3_core::setWindMagStateLearningMode() const bool haveAirspeedMeasurement = (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); if (haveAirspeedMeasurement) { trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX); - const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; - stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z); - stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z); + // handle special case where we have received prior wind speed data +#if EK3_FEATURE_SETWIND + if (lastExtWindVelSet_ms == 0) +#endif // EK3_FEATURE_SETWIND + { + const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; + stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z); + stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z); + } } else { trueAirspeedVariance = sq(WIND_VEL_VARIANCE_MAX); // use 2-sigma for faster initial convergence } @@ -91,7 +100,12 @@ void NavEKF3_core::setWindMagStateLearningMode() // set the wind state variances to the measurement uncertainty zeroCols(P, 22, 23); zeroRows(P, 22, 23); - P[22][22] = P[23][23] = trueAirspeedVariance; +#if EK3_FEATURE_SETWIND + if (lastExtWindVelSet_ms == 0) +#endif // EK3_FEATURE_SETWIND + { + P[22][22] = P[23][23] = trueAirspeedVariance; + } windStatesAligned = true; @@ -99,8 +113,14 @@ void NavEKF3_core::setWindMagStateLearningMode() // set the variances using a typical max wind speed for small UAV operation zeroCols(P, 22, 23); zeroRows(P, 22, 23); - for (uint8_t index=22; index<=23; index++) { - P[index][index] = sq(WIND_VEL_VARIANCE_MAX); +#if EK3_FEATURE_SETWIND + // preserve previously set state variances + if (lastExtWindVelSet_ms == 0) +#endif // EK3_FEATURE_SETWIND + { + for (uint8_t index=22; index<=23; index++) { + P[index][index] = sq(WIND_VEL_VARIANCE_MAX); + } } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 2fb368c60236b..fe3b877e81e40 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -414,6 +414,10 @@ void NavEKF3_core::InitialiseVariables() storedExtNavVel.reset(); #endif +#if EK3_FEATURE_SETWIND + lastExtWindVelSet_ms = 0; +#endif // EK3_FEATURE_SETWIND + // initialise pre-arm message dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising"); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index ee4d17a6f5c51..6361b348a333a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -124,6 +124,10 @@ // maximum GPs ground course uncertainty allowed for yaw alignment (deg) #define GPS_VEL_YAW_ALIGN_MAX_ANG_ERR 15.0F +#if EK3_FEATURE_POSITION_RESET +#define WIND_SPD_UNCERTAINTY 0.5F +#endif + class NavEKF3_core : public NavEKF_core_common { public: @@ -238,6 +242,14 @@ class NavEKF3_core : public NavEKF_core_common // Popoluates the WMM data structure with the field at the given location void setEarthFieldFromLocation(const Location &loc); + // Set the EKF's wind velocity states using the supplied wind speed and direction. + // speed is true speed measured in m/s + // speed_accuracy is the 1-sigma wind speed uncertainty in m/s. Set to NaN if unknown and a default value wil be used internally. + // direction is the azimuth angle in degrees from true north that the wind is coming from + // direction_accuracy is the 1-sigma direction uncertainty in deg. Set to NaN if unknown and a default value will be used internally. + // Returns true if the set was successful. + bool setWind(float speed, float speed_accuracy, float direction, float direction_accuracy); + // return estimated height above ground level // return false if ground height is not being estimated. bool getHAGL(float &HAGL) const; @@ -1487,6 +1499,10 @@ class NavEKF3_core : public NavEKF_core_common #endif // EK3_FEATURE_EXTERNAL_NAV bool useExtNavVel; // true if external nav velocity should be used +#if EK3_FEATURE_SETWIND + uint32_t lastExtWindVelSet_ms; // last system time the wind velocity states were set externally +#endif // EK3_FEATURE_SETWIND + // flags indicating severe numerical errors in innovation variance calculation for different fusion operations struct { bool bad_xmag:1; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h index 18a3c4ae61f12..13b9a783f3de4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h @@ -37,6 +37,10 @@ #define EK3_FEATURE_POSITION_RESET EK3_FEATURE_ALL || AP_AHRS_POSITION_RESET_ENABLED #endif +#ifndef EK3_FEATURE_SETWIND +#define EK3_FEATURE_SETWIND EK3_FEATURE_ALL || BOARD_FLASH_SIZE > 1024 +#endif + // rangefinder measurements if available #ifndef EK3_FEATURE_RANGEFINDER_MEASUREMENTS #define EK3_FEATURE_RANGEFINDER_MEASUREMENTS AP_RANGEFINDER_ENABLED diff --git a/libraries/AP_NavEKF3/derivation/generate_1.py b/libraries/AP_NavEKF3/derivation/generate_1.py index 8f738925c8e0e..0de482d6c4abb 100755 --- a/libraries/AP_NavEKF3/derivation/generate_1.py +++ b/libraries/AP_NavEKF3/derivation/generate_1.py @@ -511,6 +511,28 @@ def yaw_estimator(): yaw_estimator_observation_generator.write_matrix(Matrix(P_new_s), "_ekf_gsf[model_index].P", True) yaw_estimator_observation_generator.close() +def polar_wind_error_propagation(): + # polar and cartesian wind coordinates + spd, dirn, velN, velE = symbols("spd dirn velN velE", real=True) + # wind speed and direction observation variance + spdVar, dirnVar = symbols("spdVar dirnVar", real=True) + # cartesian wind coordinatres represent the wind vector, whereas polar coordinates represent + # the reciprocal vector as per measurement convention + velN = -spd*cos(dirn) + velE = -spd*sin(dirn) + J = Matrix([velN,velE]).jacobian(Matrix([spd,dirn])) + R_polar = create_symmetric_cov_matrix(2) + R_polar[0,0] = spdVar + R_polar[1,1] = dirnVar + R_polar[0,1] = 0 + R_polar[1,0] = 0 + R_cartesian = J * R_polar * J.transpose() + R_simplified = cse(R_cartesian, symbols("PS0:400"), optimizations='basic') + code_generator = CodeGenerator("./generated/polar_wind_to_cartesian_covariance.cpp") + code_generator.write_subexpressions(R_simplified[0]) + code_generator.write_matrix(Matrix(R_simplified[1]), "R_obs", False, "[", "]") + code_generator.close() + def quaternion_error_propagation(): # define quaternion state vector q0, q1, q2, q3 = symbols("q0 q1 q2 q3", real=True) @@ -678,6 +700,8 @@ def generate_code(): # derive autocode for other methods + # print('Computing polar wind error covariance matrix ...') + # polar_wind_error_propagation() # print('Computing tilt error covariance matrix ...') # quaternion_error_propagation() # print('Generating heading observation code ...') diff --git a/libraries/AP_NavEKF3/derivation/generated/polar_wind_to_cartesian_covariance.cpp b/libraries/AP_NavEKF3/derivation/generated/polar_wind_to_cartesian_covariance.cpp new file mode 100644 index 0000000000000..424d6910a71b5 --- /dev/null +++ b/libraries/AP_NavEKF3/derivation/generated/polar_wind_to_cartesian_covariance.cpp @@ -0,0 +1,14 @@ +const ftype PS0 = cosf(dirn); +const ftype PS1 = powf(PS0, 2); +const ftype PS2 = sinf(dirn); +const ftype PS3 = powf(PS2, 2); +const ftype PS4 = dirnVar*powf(spd, 2); +const ftype PS5 = PS0*PS2*(-PS4 + spdVar); + + +R_obs[0][0] = PS1*spdVar + PS3*PS4; +R_obs[1][0] = PS5; +R_obs[0][1] = PS5; +R_obs[1][1] = PS1*PS4 + PS3*spdVar; + + From b0b93efe9745b344c95349d71361440295da8509 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Oct 2024 12:45:37 +1000 Subject: [PATCH 08/16] AP_AHRS: pass external wind estimates through to EKF3 --- libraries/AP_AHRS/AP_AHRS.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 00988f1a0a6b9..504f88d67b6de 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -132,10 +132,17 @@ class AP_AHRS { } #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED - void set_external_wind_estimate(float speed, float direction) { + bool set_external_wind_estimate(float speed, float speed_accuracy, float direction, float direction_accuracy) { +#if AP_AHRS_DCM_ENABLED dcm.set_external_wind_estimate(speed, direction); - } #endif +#if HAL_NAVEKF3_AVAILABLE + EKF3.setWind(speed, speed_accuracy, direction, direction_accuracy); +#endif + // should this method return true/false based on result from current estimator? configured estimator? + return true; + } +#endif // AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED // return the parameter AHRS_WIND_MAX in metres per second uint8_t get_max_wind() const { From 89a790d3683ea89fcacbe686c3ee94f7da943b94 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Oct 2024 12:46:58 +1000 Subject: [PATCH 09/16] GCS_MAVLink: pass external airspeed speed accuracy and direction accuracy to AHRS --- libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 743b8b7e61d30..d668eb1cecbdf 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5333,7 +5333,15 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_external_wind_estimate(const mavlink_ return MAV_RESULT_DENIED; } - AP::ahrs().set_external_wind_estimate(packet.param1, packet.param3); + const float speed = packet.param1; + const float speed_accuracy = packet.param2; + const float direction = packet.param3; + const float direction_accuracy = packet.param4; + + if (!AP::ahrs().set_external_wind_estimate(speed, speed_accuracy, direction, direction_accuracy)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; } #endif // AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED From e637a1b2cb74c543cc356780e8239e8a274d5a7a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 28 Aug 2024 10:04:32 +1000 Subject: [PATCH 10/16] autotest: Add external wind set to replay message list --- Tools/autotest/vehicle_test_suite.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 08b179380d4c2..d249299285357 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -3147,7 +3147,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', 'RSLL'] + 'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH', 'RSLL', 'RSWS'] docco_ids = {} for thing in tree.logformat: From c9f56a5f4ecec743af7f3ff93143a93e6d11b13a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Oct 2024 13:29:19 +1000 Subject: [PATCH 11/16] autotest: do replay logging for external wind estimate allows perusal of logs to ensure replay is working --- Tools/autotest/arduplane.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index bc0d157258f51..08f3dd265759c 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -6191,6 +6191,12 @@ def ForceArm(self): self.disarm_vehicle() def _MAV_CMD_EXTERNAL_WIND_ESTIMATE(self, command): + self.set_parameters({ + "LOG_DISARMED": 1, + "LOG_REPLAY": 1, + "LOG_FILE_RATEMAX": 0, + "LOG_DARM_RATEMAX": 0, + }) self.reboot_sitl() def cmp_with_variance(a, b, p): From 6a45ad2f8fe1bd3cfdcb09136d0b2eba94b19b71 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Oct 2024 20:11:57 +1000 Subject: [PATCH 12/16] autotest: add test for NaNs passed to MAV_CMD_EXTERNAL_WIND_ESTIMATE handling --- Tools/autotest/arduplane.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 08f3dd265759c..eb39a862c5dbf 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -6190,6 +6190,26 @@ def ForceArm(self): ) self.disarm_vehicle() + def MAV_CMD_EXTERNAL_WIND_ESTIMATE_nans(self): + '''test supplying MAV_CMD_EXTERNAL_WIND_ESTIMATE with nans''' + self.set_parameters({ + "LOG_DISARMED": 1, + "LOG_REPLAY": 1, + "LOG_FILE_RATEMAX": 0, + "LOG_DARM_RATEMAX": 0, + }) + self.reboot_sitl() + + self.wait_ready_to_arm() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, + p1=5, # speed + p2=float("NaN"), # speed accuracy + p3=90, # direction + p4=float("NaN"), # direction accuracy + ) + self.delay_sim_time(10) + def _MAV_CMD_EXTERNAL_WIND_ESTIMATE(self, command): self.set_parameters({ "LOG_DISARMED": 1, @@ -6441,6 +6461,7 @@ def tests1b(self): self.SetHomeAltChange, self.ForceArm, self.MAV_CMD_EXTERNAL_WIND_ESTIMATE, + self.MAV_CMD_EXTERNAL_WIND_ESTIMATE_nans, self.GliderPullup, self.BadRollChannelDefined, ] From facb817b06f747fe5235c78b4f94ba72885a379a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Oct 2024 20:12:17 +1000 Subject: [PATCH 13/16] AP_NavEKF3: use accuracy in setWind only if not NaN --- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 6a379b8c40d74..3ce6c42318a04 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -768,8 +768,8 @@ bool NavEKF3_core::setWind(float speed, float speed_accuracy, float direction, f zeroCols(P,22,23); // set the wind state variances - if (is_positive(speed_accuracy)) { - if (is_positive(direction_accuracy)) { + if (!isnan(speed_accuracy) && is_positive(speed_accuracy)) { + if (!isnan(direction_accuracy) && is_positive(direction_accuracy)) { const ftype spdVar = sq(speed_accuracy); const ftype dirnVar = sq(radians(direction_accuracy)); From a72cb0308e981f21f03028dcb2ccc278035d240f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Oct 2024 11:06:07 +1000 Subject: [PATCH 14/16] autotest: add test for wind direction for externally-supplied wind --- Tools/autotest/quadplane.py | 63 +++++++++++++++++++++++++++++++++++++ 1 file changed, 63 insertions(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index a5a61fe763f79..74056ec265af9 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -10,6 +10,7 @@ import numpy import math +from pymavlink import mavextra from pymavlink import mavutil from pymavlink.rotmat import Vector3 @@ -1858,6 +1859,67 @@ def RTL_AUTOLAND_1_FROM_GUIDED(self): self.fly_home_land_and_disarm() + def MAV_CMD_EXTERNAL_WIND_ESTIMATE_direction(self): + '''ensure MAV_CMD_EXTERNAL_WIND_ESTIMATE direction aligns with ArduPilot's conventions''' + wind_from_dir = 270 # wind from the West + wind_speed = 5 + wind_direction_accuracy = 30 + wind_speed_accuracy = 1 + + self.set_parameters({ + "SIM_WIND_DIR": wind_from_dir, + "SIM_WIND_SPD": wind_speed, + # "ARSPD_USE": 0, # we instantly nuke our wind estimate if we have one of these + "LOG_DISARMED": 1, + "LOG_REPLAY": 1, + }) + self.reboot_sitl() + + self.wait_ready_to_arm() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, + p1=wind_speed, # speed + p2=wind_speed_accuracy , # speed accuracy + p3=wind_from_dir, # direction + p4=wind_direction_accuracy, # direction accuracy + ) + + class ValidateWindSpeedDir(vehicle_test_suite.TestSuite.MessageHook): + '''asserts wind is from a direction and at a speed''' + + def __init__(self, suite, speed, direction, epsilon_speed=1, epsilon_direction=30): + super(ValidateWindSpeedDir, self).__init__(suite) + self.speed = speed + self.direction = direction + self.epsilon_speed = epsilon_speed + self.epsilon_direction = epsilon_direction + + def hook_removed(self): + pass + + def process(self, mav, m): + if m.get_type() != 'WIND': + return + # check speed + self.suite.assert_message_field_values(m, { + "speed": self.speed + }, epsilon=self.epsilon_direction) + # check direction + self.suite.assert_message_field_values(m, { + "direction": mavextra.wrap_180(self.direction), + }, epsilon=self.epsilon_direction) + + # self.install_message_hook_context(ValidateWindSpeedDir(self, wind_speed, wind_from_dir)) + + self.arm_vehicle() + self.change_mode('QHOVER') + self.set_rc(3, 1800) + self.delay_sim_time(30) + # self.send_debug_trap() + self.change_mode('FBWA') + self.delay_sim_time(30) + self.fly_home_land_and_disarm(timeout=600) + def tests(self): '''return list of all tests''' @@ -1872,6 +1934,7 @@ def tests(self): self.TestLogDownload, self.TestLogDownloadWrap, self.EXTENDED_SYS_STATE, + self.MAV_CMD_EXTERNAL_WIND_ESTIMATE_direction, self.Mission, self.Weathervane, self.QAssist, From ada5c3f4a04c560d13f2d479d667a82ca5e69d9b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Oct 2024 11:09:18 +1000 Subject: [PATCH 15/16] AP_AHRS: DCM: tidy variable creation --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d64dc1b628784..eb10db1b45880 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1007,11 +1007,10 @@ void AP_AHRS_DCM::estimate_wind(void) if (diff_length > 0.2f) { // when turning, use the attitude response to estimate // wind speed - float V; const Vector3f velocityDiff = velocity - _last_vel; // estimate airspeed it using equation 6 - V = velocityDiff.length() / diff_length; + const float V = velocityDiff.length() / diff_length; const Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse; const Vector3f velocitySum = velocity + _last_vel; @@ -1023,10 +1022,11 @@ void AP_AHRS_DCM::estimate_wind(void) const float sintheta = sinf(theta); const float costheta = cosf(theta); - Vector3f wind = Vector3f(); - wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y); - wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y); - wind.z = velocitySum.z - V * fuselageDirectionSum.z; + Vector3f wind{ + velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y), + velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y), + velocitySum.z - V * fuselageDirectionSum.z + }; wind *= 0.5f; if (wind.length() < _wind.length() + 20) { From 76b893a68f201445cf9cce5d2dc49bab357de3a2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 8 Oct 2024 12:50:54 +1100 Subject: [PATCH 16/16] autotest: add simple test that wind estimates from DCM and EKF3 converge --- Tools/autotest/quadplane.py | 53 +++++++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 74056ec265af9..8a17f910a766e 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1920,6 +1920,58 @@ def process(self, mav, m): self.delay_sim_time(30) self.fly_home_land_and_disarm(timeout=600) + def WindEstimateConsistency(self): + '''test that DCM and EKF3 roughly agree on wind speed and direction''' + self.set_parameters({ + 'SIM_WIND_SPD': 10, # metres/second + 'SIM_WIND_DIR': 315, # from the North-West + }) + self.change_mode('TAKEOFF') + self.wait_ready_to_arm() + self.arm_vehicle() + self.delay_sim_time(180) + mlog = self.dfreader_for_current_onboard_log() + self.fly_home_land_and_disarm() + + self.progress("Inspecting dataflash log") + match_start_time = None + dcm = None + xkf2 = None + while True: + m = mlog.recv_match( + type=['DCM', 'XKF2'], + blocking=True, + ) + if m is None: + raise NotAchievedException("Did not see wind estimates match") + + m_type = m.get_type() + if m_type == 'DCM': + dcm = m + else: + xkf2 = m + if dcm is None or xkf2 is None: + continue + + now = m.TimeUS * 1e-6 + + matches_east = abs(dcm.VWE-xkf2.VWE) < 1.5 + matches_north = abs(dcm.VWN-xkf2.VWN) < 1.5 + + matches = matches_east and matches_north + + if not matches: + match_start_time = None + continue + + if match_start_time is None: + match_start_time = now + continue + + if now - match_start_time > 60: + self.progress("Wind estimates correlated") + break + def tests(self): '''return list of all tests''' @@ -1948,6 +2000,7 @@ def tests(self): self.GUIDEDToAUTO, self.BootInAUTO, self.Ship, + self.WindEstimateConsistency, self.MAV_CMD_NAV_LOITER_TO_ALT, self.LoiterAltQLand, self.VTOLLandSpiral,