Allow setting of EKF3 wind state from MAV_CMD_EXTERNAL_WIND_ESTIMATE

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);

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;

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);
  }
 

Tools/autotest/arduplane.py

@@ -6130,6 +6130,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):

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:

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 {

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 {

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;

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", "----", "----" },

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<num_cores; i++) {
+ ret |= core[i].setWind(speed, speed_accuracy, direction, direction_accuracy);
+ }
+ // return true if any core accepts the new wind speed
+ return ret;
+}
+#endif // EK3_FEATURE_SETWIND
 
 // return estimated height above ground level
 // return false if ground height is not being estimated.

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -157,6 +157,14 @@ class NavEKF3 {
  // Returns true if the set was successful
  bool setLatLng(const Location &loc, float posErr, uint32_t timestamp_ms);
 
+ // 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;

libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@@ -760,3 +760,60 @@ void NavEKF3_core::FuseDragForces()
 * MISC FUNCTIONS *
 ********************************************************/
 
+#if EK3_FEATURE_SETWIND
+bool NavEKF3_core::setWind(float speed, float speed_accuracy, float direction, float direction_accuracy)
+{
+ // reset the corresponding covariances
+ zeroRows(P,22,23);
+ zeroCols(P,22,23);
+
+ // set the wind state variances
+ if (is_positive(speed_accuracy)) {
+ if (is_positive(direction_accuracy)) {
+ const ftype spdVar = sq(speed_accuracy);
+ const ftype dirnVar = sq(radians(direction_accuracy));
+
+ const ftype PS0 = cosf(radians(direction));
+ const ftype PS1 = sq(PS0);
+ const ftype PS2 = sinf(radians(direction));
+ const ftype PS3 = sq(PS2);
+ const ftype PS4 = dirnVar*sq(speed);
+ const ftype PS5 = PS0*PS2*(-PS4 + spdVar);
+
+ P[22][22] = PS1*spdVar + PS3*PS4;
+ P[23][22] = PS5;
+ P[22][23] = PS5;
+ P[23][23] = PS1*PS4 + PS3*spdVar;
+ } else {
+ P[22][22] = P[23][23] = sq(speed_accuracy);
+ }
+ } else {
+ P[22][22] = P[23][23] = sq(WIND_SPD_UNCERTAINTY);
+ }
+
+ // reset the NE wind velocity states to the measurement
+ Vector2F wind_vel_prev = stateStruct.wind_vel;
+ stateStruct.wind_vel.x = -speed * cosF(radians(direction));
+ stateStruct.wind_vel.y = -speed * sinF(radians(direction));
+
+ // update the vehicle velocity states to be consistent with the new wind estimate if dead reckoning
+ if ((imuSampleTime_ms - lastGpsPosPassTime_ms) > frontend->deadReckonDeclare_ms) {
+ velResetNE = stateStruct.wind_vel - wind_vel_prev;
+ stateStruct.velocity.xy() += velResetNE;
+
+ // update the output buffer
+ for (uint8_t i=0; i<imu_buffer_length; i++) {
+ storedOutput[i].velocity.xy() = stateStruct.velocity.xy();
+ }
+ outputDataNew.velocity.xy() = stateStruct.velocity.xy();
+ outputDataDelayed.velocity.xy() = stateStruct.velocity.xy();
+
+ // store the time of the reset
+ lastVelReset_ms = imuSampleTime_ms;
+ lastExtWindVelSet_ms = imuSampleTime_ms;
+ }
+
+ return true;
+
+}
+#endif // EK3_FEATURE_SETWIND

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -66,6 +66,9 @@ void NavEKF3_core::setWindMagStateLearningMode()
  inhibitWindStates = true;
  lastAspdEstIsValid = false;
  updateStateIndexLim();
+#if EK3_FEATURE_SETWIND
+ lastExtWindVelSet_ms = 0;
+#endif // EK3_FEATURE_SETWIND
  } else if (inhibitWindStates && canEstimateWind &&
  (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
  inhibitWindStates = false;
@@ -81,26 +84,43 @@ 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
  }
 
  // 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;
 
  } else {
  // 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);
+ }
  }
  }
  }

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");
 

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;

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