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

msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge

Browse Source 
- update all msgs to be directly compatible with ROS2
 - microdds_client improvements
   - timesync
   - reduced code size
   - add to most default builds if we can afford it
   - lots of other little changes
 - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)
This commit is contained in:
Daniel Agar
2022-10-19 19:36:47 -04:00
committed by GitHub
parent e211e0ca0e
commit cea185268e
318 changed files with 1948 additions and 8939 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+2 -2
View File
@@ -49,7 +49,7 @@
#include <mathlib/mathlib.h>
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const
{
// reset flags
resetEstimatorAidStatus(airspeed);
@@ -75,7 +75,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
setEstimatorAidStatusTestRatio(airspeed, innov_gate);
}
void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed)
{
if (airspeed.innovation_rejected) {
return;
src/modules/ekf2/EKF/ekf.h
+38 -38
View File
@@ -49,9 +49,9 @@
#include "bias_estimator.hpp"
#include "height_bias_estimator.hpp"
#include <uORB/topics/estimator_aid_source_1d.h>
#include <uORB/topics/estimator_aid_source_2d.h>
#include <uORB/topics/estimator_aid_source_3d.h>
#include <uORB/topics/estimator_aid_source1d.h>
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/estimator_aid_source3d.h>
enum class Likelihood { LOW, MEDIUM, HIGH };
@@ -551,30 +551,30 @@ private:
uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
estimator_aid_source_1d_s _aid_src_baro_hgt{};
estimator_aid_source_1d_s _aid_src_rng_hgt{};
estimator_aid_source_1d_s _aid_src_airspeed{};
estimator_aid_source_1d_s _aid_src_sideslip{};
estimator_aid_source1d_s _aid_src_baro_hgt{};
estimator_aid_source1d_s _aid_src_rng_hgt{};
estimator_aid_source1d_s _aid_src_airspeed{};
estimator_aid_source1d_s _aid_src_sideslip{};
estimator_aid_source_2d_s _aid_src_fake_pos{};
estimator_aid_source_1d_s _aid_src_fake_hgt{};
estimator_aid_source2d_s _aid_src_fake_pos{};
estimator_aid_source1d_s _aid_src_fake_hgt{};
estimator_aid_source_1d_s _aid_src_ev_hgt{};
estimator_aid_source_2d_s _aid_src_ev_pos{};
estimator_aid_source_3d_s _aid_src_ev_vel{};
estimator_aid_source_1d_s _aid_src_ev_yaw{};
estimator_aid_source1d_s _aid_src_ev_hgt{};
estimator_aid_source2d_s _aid_src_ev_pos{};
estimator_aid_source3d_s _aid_src_ev_vel{};
estimator_aid_source1d_s _aid_src_ev_yaw{};
estimator_aid_source_1d_s _aid_src_gnss_hgt{};
estimator_aid_source_2d_s _aid_src_gnss_pos{};
estimator_aid_source_3d_s _aid_src_gnss_vel{};
estimator_aid_source_1d_s _aid_src_gnss_yaw{};
estimator_aid_source1d_s _aid_src_gnss_hgt{};
estimator_aid_source2d_s _aid_src_gnss_pos{};
estimator_aid_source3d_s _aid_src_gnss_vel{};
estimator_aid_source1d_s _aid_src_gnss_yaw{};
estimator_aid_source_1d_s _aid_src_mag_heading{};
estimator_aid_source_3d_s _aid_src_mag{};
estimator_aid_source1d_s _aid_src_mag_heading{};
estimator_aid_source3d_s _aid_src_mag{};
estimator_aid_source_2d_s _aid_src_aux_vel{};
estimator_aid_source2d_s _aid_src_aux_vel{};
estimator_aid_source_2d_s _aid_src_optical_flow{};
estimator_aid_source2d_s _aid_src_optical_flow{};
// output predictor states
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
@@ -662,12 +662,12 @@ private:
void predictCovariance();
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true);
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
// innovation : prediction - measurement
// variance : observaton variance
bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s &aid_src_status);
bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw(const gpsSample &gps_sample);
@@ -683,12 +683,12 @@ private:
// apply sensible limits to the declination and length of the NE mag field states estimates
void limitDeclination();
void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const;
void fuseAirspeed(estimator_aid_source_1d_s &airspeed);
void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const;
void fuseAirspeed(estimator_aid_source1d_s &airspeed);
// fuse synthetic zero sideslip measurement
void updateSideslip(estimator_aid_source_1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source_1d_s &_aid_src_sideslip);
void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(const dragSample &drag_sample);
@@ -718,24 +718,24 @@ private:
void resetVerticalVelocityToZero();
// fuse optical flow line of sight rate measurements
void updateOptFlow(estimator_aid_source_2d_s &aid_src);
void updateOptFlow(estimator_aid_source2d_s &aid_src);
void fuseOptFlow();
// horizontal and vertical position aid source
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const;
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source_1d_s &aid_src) const;
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
// 2d & 3d velocity aid source
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source_3d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
// horizontal and vertical position fusion
void fuseHorizontalPosition(estimator_aid_source_2d_s &pos_aid_src);
void fuseVerticalPosition(estimator_aid_source_1d_s &hgt_aid_src);
void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
// 2d & 3d velocity fusion
void fuseVelocity(estimator_aid_source_2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source_3d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
void updateGpsYaw(const gpsSample &gps_sample);
@@ -1085,7 +1085,7 @@ private:
void resetGpsDriftCheckFilters();
void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const
void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
{
// only bother resetting if timestamp_sample is set
if (status.timestamp_sample != 0) {
@@ -1130,7 +1130,7 @@ private:
}
}
void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const
void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
{
if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) {
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
src/modules/ekf2/EKF/mag_fusion.cpp
+2 -2
View File
@@ -50,7 +50,7 @@
#include <mathlib/mathlib.h>
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states)
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states)
{
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
@@ -223,7 +223,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b
}
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status)
bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status)
{
aid_src_status.innovation = innovation;
src/modules/ekf2/EKF/optflow_fusion.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <float.h>
#include "utils.hpp"
void Ekf::updateOptFlow(estimator_aid_source_2d_s &aid_src)
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
{
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = _flow_sample_delayed.time_us;
src/modules/ekf2/EKF/sideslip_fusion.cpp
+2 -2
View File
@@ -47,7 +47,7 @@
#include <mathlib/mathlib.h>
void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const
void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
{
// reset flags
resetEstimatorAidStatus(sideslip);
@@ -71,7 +71,7 @@ void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const
setEstimatorAidStatusTestRatio(sideslip, innov_gate);
}
void Ekf::fuseSideslip(estimator_aid_source_1d_s &sideslip)
void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
{
if (sideslip.innovation_rejected) {
return;
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+8 -8
View File
@@ -44,7 +44,7 @@
#include "ekf.h"
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source_2d_s &aid_src) const
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -62,7 +62,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob
}
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
const float innov_gate, estimator_aid_source_3d_s &aid_src) const
const float innov_gate, estimator_aid_source3d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -88,7 +88,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
}
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
const float innov_gate, estimator_aid_source_1d_s &aid_src) const
const float innov_gate, estimator_aid_source1d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -112,7 +112,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa
}
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source_2d_s &aid_src) const
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
@@ -129,7 +129,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src)
void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
{
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
// vx, vy
@@ -145,7 +145,7 @@ void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src)
}
}
void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src)
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
// vx, vy, vz
@@ -162,7 +162,7 @@ void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src)
}
}
void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src)
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
@@ -178,7 +178,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src)
}
}
void Ekf::fuseVerticalPosition(estimator_aid_source_1d_s &aid_src)
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+3 -3
View File
@@ -47,7 +47,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
float innovation = 0.f;
float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
estimator_aid_source_1d_s unused;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
_last_static_yaw = NAN;
@@ -62,7 +62,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = wrap_pi(euler_yaw - _last_static_yaw);
float obs_var = 0.01f;
estimator_aid_source_1d_s unused;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
@@ -78,7 +78,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = 0.f;
float obs_var = 0.01f;
estimator_aid_source_1d_s unused;
estimator_aid_source1d_s unused;
fuseYaw(innovation, obs_var, unused);
}
src/modules/ekf2/EKF2.hpp
+18 -18
View File
@@ -353,30 +353,30 @@ private:
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;