diff --git a/docs/zh/msg_docs/EstimatorStatusFlags.md b/docs/zh/msg_docs/EstimatorStatusFlags.md index 19a6a00d23..e3e33c6a39 100644 --- a/docs/zh/msg_docs/EstimatorStatusFlags.md +++ b/docs/zh/msg_docs/EstimatorStatusFlags.md @@ -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 diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index b7933afdec..cf565e02d3 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -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 diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index f22e2b727c..ef85dbaf7c 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -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 diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 669a4b8af9..8f20ec2d85 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -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. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 169cc34587..cb5e87ff05 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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(); diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index d178bd8ebf..d03229f7c2 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -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; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index de4eb07ad6..63655750f5 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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; }; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b421bcb889..bbabed8bc0 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -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}; diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 9560d8110f..fd723613dd 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -37,7 +37,7 @@ #include -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); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b40601bdd0..c409a3ef47 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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 ×tamp) 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;