fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: support heading external update from MAV_CMD_EXTERNAL_ATTITUDE_ESTIMATE

Browse Source
This commit is contained in:
bresch 2025-09-03 10:36:13 +02:00 committed by Mathieu Bresciani
parent eba0b99950
commit 82308da18d
10 changed files with 106 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
docs/zh/msg_docs/EstimatorStatusFlags.md
View File

@ -54,6 +54,7 @@ bool cs_valid_fake_pos # 41 - true if a valid constant position is bein
bool cs_constant_pos # 42 - true if the vehicle is at a constant position
bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used
bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended
bool cs_yaw_manual # 45 - true if yaw has been set manually
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

1
msg/EstimatorStatus.msg
View File

@ -49,6 +49,7 @@ uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared
uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used
uint8 CS_GNSS_VEL = 44 # 44 - true if GNSS velocity measurement fusion is intended
uint8 CS_GNSS_FAULT = 45 # 45 - true if GNSS measurements have been declared faulty and are no longer used
uint8 CS_YAW_MANUAL = 46 # 46 - true if yaw has been set manually
uint32 filter_fault_flags # Bitmask to indicate EKF internal faults
# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error

1
msg/EstimatorStatusFlags.msg
View File

@ -50,6 +50,7 @@ bool cs_constant_pos # 42 - true if the vehicle is at a constant posi
bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used
bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended
bool cs_gnss_fault # 45 - true if GNSS measurements have been declared faulty and are no longer used
bool cs_yaw_manual # 46 - true if yaw has been set manually
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

1
msg/versioned/VehicleCommand.msg
View File

@ -88,6 +88,7 @@ uint16 VEHICLE_CMD_REQUEST_CAMERA_INFORMATION = 521 # Request camera information
uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.).
uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom.
uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
uint16 VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE = 620 # Set an external estimate of vehicle attitude in degrees.
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw.
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control.
uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence.

19
src/modules/commander/Commander.cpp
View File

@ -462,6 +462,22 @@ int Commander::custom_command(int argc, char *argv[])
}
}
if (!strcmp(argv[0], "set_heading")) {
if (argc > 1) {
const float heading = atof(argv[1]);
const float heading_accuracy = NAN;
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE,
0.f, 0.f, heading, 0.f, 0.f, 0.f, heading_accuracy);
return (ret ? 0 : 1);
} else {
PX4_ERR("missing argument");
return 0;
}
}
if (!strcmp(argv[0], "poweroff")) {
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
@ -1521,6 +1537,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE:
/* ignore commands that are handled by other parts of the system */
break;
@ -3040,6 +3057,8 @@ The commander module contains the state machine for mode switching and failsafe
PRINT_MODULE_USAGE_COMMAND("set_ekf_origin");
PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude");
PRINT_MODULE_USAGE_COMMAND_DESCR("set_heading", "Set current heading");
PRINT_MODULE_USAGE_ARG("heading", "degrees from True North [0 360]", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("poweroff", "Power off board (if supported)");
#endif
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

8
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -181,7 +181,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gnss_yaw;
&& !_control_status.flags.gnss_yaw
&& (!_control_status.flags.yaw_manual || _control_status.flags.mag_aligned_in_flight);
_control_status.flags.mag_3D = common_conditions_passing
&& (_params.ekf2_mag_type == MagFuseType::AUTO)
@ -210,7 +211,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D))
if ((checkHaglYawResetReq() && (_control_status.flags.mag_hdg || _control_status.flags.mag_3D
|| _control_status.flags.yaw_manual))
|| (wmm_updated && no_ne_aiding_or_not_moving)) {
ECL_INFO("reset to %s", AID_SRC_NAME);
const bool reset_heading = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
@ -448,7 +450,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
}
// record the start time for the magnetic field alignment
if (_control_status.flags.in_air && reset_heading) {
if (_control_status.flags.in_air && (reset_heading || _control_status.flags.yaw_manual)) {
_control_status.flags.mag_aligned_in_flight = true;
_flt_mag_align_start_time = _time_delayed_us;
}

1
src/modules/ekf2/EKF/common.h
View File

@ -608,6 +608,7 @@ uint64_t mag_heading_consistent :
uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
uint64_t gnss_fault :
1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used
uint64_t yaw_manual : 1; ///< 46 - true if yaw has been reset manually
} flags;
uint64_t value;
};

22
src/modules/ekf2/EKF/ekf.h
View File

@ -416,6 +416,22 @@ public:
bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
uint64_t timestamp_observation);
void resetHeadingToExternalObservation(float heading, float heading_accuracy)
{
if (_control_status.flags.yaw_align) {
resetYawByFusion(heading, heading_accuracy);
} else {
resetQuatStateYaw(heading, heading_accuracy);
_control_status.flags.yaw_align = true;
}
// Force the mag consistency check to pass again since an external heading reset is often done to
// counter mag disturbances.
_control_status.flags.mag_heading_consistent = false;
_control_status.flags.yaw_manual = true;
}
void updateParameters();
friend class AuxGlobalPosition;
@ -647,8 +663,8 @@ private:
bool initialiseAltitudeTo(float altitude, float vpos_var = NAN);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW, const bool reset = false);
void computeYawInnovVarAndH(float observation_variance, float &innovation_variance, VectorState &H_YAW) const;
void updateIMUBiasInhibit(const imuSample &imu_delayed);
@ -999,6 +1015,8 @@ private:
// yaw : Euler yaw angle (rad)
// yaw_variance : yaw error variance (rad^2)
void resetQuatStateYaw(float yaw, float yaw_variance);
void propagateQuatReset(const Quatf &quat_before_reset);
void resetYawByFusion(float yaw, float yaw_variance);
HeightSensor _height_sensor_ref{HeightSensor::UNKNOWN};
PositionSensor _position_sensor_ref{PositionSensor::GNSS};

49
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -37,7 +37,7 @@
#include <mathlib/mathlib.h>
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW)
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW, bool reset)
{
// check if the innovation variance calculation is badly conditioned
if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
@ -68,6 +68,11 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
Kfusion(row) *= heading_innov_var_inv;
}
if (reset && fabsf(H_YAW(State::quat_nominal.idx + 2)) > FLT_EPSILON) {
// Reset the yaw estimate by forcing the measurement into the state
Kfusion(State::quat_nominal.idx + 2) = 1.f / H_YAW(State::quat_nominal.idx + 2);
}
// set the heading unhealthy if the test fails
if (aid_src_status.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
@ -110,12 +115,12 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
return true;
}
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const
void Ekf::computeYawInnovVarAndH(float observation_variance, float &innovation_variance, VectorState &H_YAW) const
{
sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
sym::ComputeYawInnovVarAndH(_state.vector(), P, observation_variance, &innovation_variance, &H_YAW);
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
void Ekf::resetQuatStateYaw(const float yaw, const float yaw_variance)
{
// save a copy of the quaternion state for later use in calculating the amount of reset change
const Quatf quat_before_reset = _state.quat_nominal;
@ -129,12 +134,17 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
// update the rotation matrix using the new yaw value
_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
// calculate the amount that the quaternion has changed by
const Quatf quat_after_reset(_R_to_earth);
const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
// update quaternion states
_state.quat_nominal = quat_after_reset;
_state.quat_nominal = Quatf(_R_to_earth);
_time_last_heading_fuse = _time_delayed_us;
propagateQuatReset(quat_before_reset);
}
void Ekf::propagateQuatReset(const Quatf &quat_before_reset)
{
const Quatf q_error((_state.quat_nominal * quat_before_reset.inversed()).normalized());
// add the reset amount to the output observer buffered data
_output_predictor.resetQuaternion(q_error);
@ -160,6 +170,23 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
}
_state_reset_status.reset_count.quat++;
_time_last_heading_fuse = _time_delayed_us;
}
void Ekf::resetYawByFusion(const float yaw, const float yaw_variance)
{
const Quatf quat_before_reset = _state.quat_nominal;
estimator_aid_source1d_s aid_src_status{};
aid_src_status.observation = yaw;
aid_src_status.observation_variance = yaw_variance;
aid_src_status.innovation = wrap_pi(getEulerYaw(_state.quat_nominal) - yaw);
VectorState H_YAW;
computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
const bool reset_yaw = true;
fuseYaw(aid_src_status, H_YAW, reset_yaw);
propagateQuatReset(quat_before_reset);
}

19
src/modules/ekf2/EKF2.cpp
View File

@ -580,6 +580,24 @@ void EKF2::Run()
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE) {
if (PX4_ISFINITE(vehicle_command.param3)) {
const float heading = wrap_pi(math::radians(vehicle_command.param3));
static constexpr float kDefaultHeadingAccuracyDeg = 20.f;
const float heading_accuracy = math::radians(PX4_ISFINITE(vehicle_command.param7)
? vehicle_command.param7
: kDefaultHeadingAccuracyDeg);
_ekf.resetHeadingToExternalObservation(heading, heading_accuracy);
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
}
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
}
}
@ -1936,6 +1954,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault;
status_flags.cs_gnss_vel = _ekf.control_status_flags().gnss_vel;
status_flags.cs_gnss_fault = _ekf.control_status_flags().gnss_fault;
status_flags.cs_yaw_manual = _ekf.control_status_flags().yaw_manual;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;