forked from Archive/PX4-Autopilot
Update ecl to add ability to recover from bad magnetic yaw
* msg: Add EKF-GSF yaw estimator logging data * ecl: update to version with EKF-GSF yaw estimator * ekf2: Add param control and logging for EKF-GSF yaw estimator * logger: Add logging for EKF-GSF yaw esimtator
This commit is contained in:
parent
422e89b1c0
commit
ab92b46e69
|
@ -55,7 +55,7 @@ px4_add_board(
|
||||||
navigator
|
navigator
|
||||||
rc_update
|
rc_update
|
||||||
sensors
|
sensors
|
||||||
sih
|
#sih
|
||||||
#temperature_compensation
|
#temperature_compensation
|
||||||
vmount
|
vmount
|
||||||
SYSTEMCMDS
|
SYSTEMCMDS
|
||||||
|
|
|
@ -168,6 +168,7 @@ set(msg_files
|
||||||
vtol_vehicle_status.msg
|
vtol_vehicle_status.msg
|
||||||
wheel_encoders.msg
|
wheel_encoders.msg
|
||||||
wind_estimate.msg
|
wind_estimate.msg
|
||||||
|
yaw_estimator_status.msg
|
||||||
)
|
)
|
||||||
|
|
||||||
set(deprecated_msgs
|
set(deprecated_msgs
|
||||||
|
|
|
@ -299,6 +299,8 @@ rtps:
|
||||||
id: 133
|
id: 133
|
||||||
- msg: orb_test_large
|
- msg: orb_test_large
|
||||||
id: 134
|
id: 134
|
||||||
|
- msg: yaw_estimator_status
|
||||||
|
id: 135
|
||||||
########## multi topics: begin ##########
|
########## multi topics: begin ##########
|
||||||
- msg: actuator_controls_0
|
- msg: actuator_controls_0
|
||||||
id: 150
|
id: 150
|
||||||
|
|
|
@ -0,0 +1,7 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
float32 yaw_composite # composite yaw from GSF (rad)
|
||||||
|
float32 yaw_variance # composite yaw variance from GSF (rad^2)
|
||||||
|
float32[5] yaw # yaw estimate for each model in the filter bank (rad)
|
||||||
|
float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s)
|
||||||
|
float32[5] innov_ve # East velocity innovation for each model in the filter bank (m/s)
|
||||||
|
float32[5] weight # weighting for each model in the filter bank
|
|
@ -1 +1 @@
|
||||||
Subproject commit 230c865fa9c02b07b0371df050b339bc37ce0c29
|
Subproject commit 975060d108e901f3ea70a9b88d1e5fa2112e49ff
|
|
@ -78,6 +78,7 @@
|
||||||
#include <uORB/topics/vehicle_odometry.h>
|
#include <uORB/topics/vehicle_odometry.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/wind_estimate.h>
|
#include <uORB/topics/wind_estimate.h>
|
||||||
|
#include <uORB/topics/yaw_estimator_status.h>
|
||||||
|
|
||||||
#include "Utility/PreFlightChecker.hpp"
|
#include "Utility/PreFlightChecker.hpp"
|
||||||
|
|
||||||
|
@ -130,8 +131,9 @@ private:
|
||||||
template<typename Param>
|
template<typename Param>
|
||||||
bool update_mag_decl(Param &mag_decl_param);
|
bool update_mag_decl(Param &mag_decl_param);
|
||||||
|
|
||||||
bool publish_attitude(const hrt_abstime &now);
|
void publish_attitude(const hrt_abstime ×tamp);
|
||||||
bool publish_wind_estimate(const hrt_abstime ×tamp);
|
void publish_wind_estimate(const hrt_abstime ×tamp);
|
||||||
|
void publish_yaw_estimator_status(const hrt_abstime ×tamp);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
|
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
|
||||||
|
@ -274,6 +276,7 @@ private:
|
||||||
uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
||||||
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
|
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
|
||||||
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
|
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
|
||||||
|
uORB::Publication<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||||
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
|
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
|
||||||
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
|
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
|
||||||
uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
|
uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
|
||||||
|
@ -525,7 +528,11 @@ private:
|
||||||
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
|
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
|
||||||
|
|
||||||
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
|
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
|
||||||
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check ///< Mag field strength check
|
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
|
||||||
|
|
||||||
|
// Used by EKF-GSF experimental yaw estimator
|
||||||
|
(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
|
||||||
|
_param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation
|
||||||
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
@ -637,7 +644,8 @@ Ekf2::Ekf2(bool replay_mode):
|
||||||
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
|
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
|
||||||
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
|
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
|
||||||
_param_ekf2_move_test(_params->is_moving_scaler),
|
_param_ekf2_move_test(_params->is_moving_scaler),
|
||||||
_param_ekf2_mag_check(_params->check_mag_strength)
|
_param_ekf2_mag_check(_params->check_mag_strength),
|
||||||
|
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
|
||||||
{
|
{
|
||||||
// initialise parameter cache
|
// initialise parameter cache
|
||||||
updateParams();
|
updateParams();
|
||||||
|
@ -1598,6 +1606,8 @@ void Ekf2::Run()
|
||||||
|
|
||||||
publish_wind_estimate(now);
|
publish_wind_estimate(now);
|
||||||
|
|
||||||
|
publish_yaw_estimator_status(now);
|
||||||
|
|
||||||
if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
|
if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
|
||||||
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
|
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
|
||||||
}
|
}
|
||||||
|
@ -1749,12 +1759,12 @@ int Ekf2::getRangeSubIndex()
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Ekf2::publish_attitude(const hrt_abstime &now)
|
void Ekf2::publish_attitude(const hrt_abstime ×tamp)
|
||||||
{
|
{
|
||||||
if (_ekf.attitude_valid()) {
|
if (_ekf.attitude_valid()) {
|
||||||
// generate vehicle attitude quaternion data
|
// generate vehicle attitude quaternion data
|
||||||
vehicle_attitude_s att;
|
vehicle_attitude_s att;
|
||||||
att.timestamp = now;
|
att.timestamp = timestamp;
|
||||||
|
|
||||||
const Quatf q{_ekf.calculate_quaternion()};
|
const Quatf q{_ekf.calculate_quaternion()};
|
||||||
q.copyTo(att.q);
|
q.copyTo(att.q);
|
||||||
|
@ -1763,19 +1773,33 @@ bool Ekf2::publish_attitude(const hrt_abstime &now)
|
||||||
|
|
||||||
_att_pub.publish(att);
|
_att_pub.publish(att);
|
||||||
|
|
||||||
return true;
|
|
||||||
|
|
||||||
} else if (_replay_mode) {
|
} else if (_replay_mode) {
|
||||||
// in replay mode we have to tell the replay module not to wait for an update
|
// in replay mode we have to tell the replay module not to wait for an update
|
||||||
// we do this by publishing an attitude with zero timestamp
|
// we do this by publishing an attitude with zero timestamp
|
||||||
vehicle_attitude_s att{};
|
vehicle_attitude_s att{};
|
||||||
_att_pub.publish(att);
|
_att_pub.publish(att);
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp)
|
void Ekf2::publish_yaw_estimator_status(const hrt_abstime ×tamp)
|
||||||
|
{
|
||||||
|
yaw_estimator_status_s yaw_est_test_data{};
|
||||||
|
|
||||||
|
static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
|
||||||
|
"yaw_estimator_status_s::yaw wrong size");
|
||||||
|
|
||||||
|
if (_ekf.getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance,
|
||||||
|
&yaw_est_test_data.yaw[0],
|
||||||
|
&yaw_est_test_data.innov_vn[0], &yaw_est_test_data.innov_ve[0],
|
||||||
|
&yaw_est_test_data.weight[0])) {
|
||||||
|
|
||||||
|
yaw_est_test_data.timestamp = timestamp;
|
||||||
|
|
||||||
|
_yaw_est_pub.publish(yaw_est_test_data);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf2::publish_wind_estimate(const hrt_abstime ×tamp)
|
||||||
{
|
{
|
||||||
if (_ekf.get_wind_status()) {
|
if (_ekf.get_wind_status()) {
|
||||||
// Publish wind estimate only if ekf declares them valid
|
// Publish wind estimate only if ekf declares them valid
|
||||||
|
@ -1796,11 +1820,7 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp)
|
||||||
wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
|
wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
|
||||||
|
|
||||||
_wind_pub.publish(wind_estimate);
|
_wind_pub.publish(wind_estimate);
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Ekf2::blend_gps_data()
|
bool Ekf2::blend_gps_data()
|
||||||
|
|
|
@ -1430,3 +1430,15 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
|
||||||
* @boolean
|
* @boolean
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0);
|
PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Default value of true airspeed used in EKF-GSF AHRS calculation.
|
||||||
|
* If no airspeed measurements are avalable, the EKF-GSF AHRS calculation will assume this value of true airspeed when compensating for centripetal acceleration during turns. Set to zero to disable centripetal acceleration compensation during fixed wing flight modes.
|
||||||
|
*
|
||||||
|
* @group EKF2
|
||||||
|
* @min 0.0
|
||||||
|
* @unit m/s
|
||||||
|
* @max 100.0
|
||||||
|
* @decimal 1
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
|
||||||
|
|
|
@ -95,6 +95,7 @@ void LoggedTopics::add_default_topics()
|
||||||
add_topic("vehicle_status", 200);
|
add_topic("vehicle_status", 200);
|
||||||
add_topic("vehicle_status_flags");
|
add_topic("vehicle_status_flags");
|
||||||
add_topic("vtol_vehicle_status", 200);
|
add_topic("vtol_vehicle_status", 200);
|
||||||
|
add_topic("yaw_estimator_status", 200);
|
||||||
|
|
||||||
// multi topics
|
// multi topics
|
||||||
add_topic_multi("actuator_outputs", 100);
|
add_topic_multi("actuator_outputs", 100);
|
||||||
|
|
Loading…
Reference in New Issue