From 1c49812b174dc72725a20af93be1c070bce02026 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 28 Jun 2023 11:08:19 +0200 Subject: [PATCH] add fake ev attitude error --- src/modules/fake_ev/fake_ev.cpp | 33 +++++++++++++++++++++++++-------- src/modules/fake_ev/fake_ev.hpp | 8 ++++++-- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/src/modules/fake_ev/fake_ev.cpp b/src/modules/fake_ev/fake_ev.cpp index c85fad4bab..0fa2ffe108 100644 --- a/src/modules/fake_ev/fake_ev.cpp +++ b/src/modules/fake_ev/fake_ev.cpp @@ -80,6 +80,14 @@ void FakeEV::Run() perf_begin(_cycle_perf); + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s att; + + if (_vehicle_attitude_sub.copy(&att)) { + _q_att = matrix::Quatf(att.q); + } + } + sensor_gps_s gps{}; if (_vehicle_gps_position_sub.copy(&gps)) { @@ -104,6 +112,8 @@ vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps) { vehicle_odometry_s odom{vehicle_odometry_empty}; + const matrix::Quatf q_error(matrix::Eulerf(0.f, 0.f, M_PI_F / 8.f)); + /* odom.timestamp_sample = gps.timestamp_sample; */ // gps timestamp_sample isn't set in SITL odom.timestamp_sample = gps.timestamp; @@ -113,23 +123,25 @@ vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps) if (gps.fix_type >= 3) { if (PX4_ISFINITE(_alt_ref)) { - matrix::Vector2f hpos = _hpos_prev; + matrix::Vector3f pos = _pos_prev; if (!_param_fev_stale.get()) { - hpos = _pos_ref.project(gps.lat / 1.0e7, gps.lon / 1.0e7); - _hpos_prev = hpos; + matrix::Vector2f hpos = _pos_ref.project(gps.lat / 1.0e7, gps.lon / 1.0e7); + const float vpos = -((float)gps.alt * 1e-3f - _alt_ref); + pos = matrix::Vector3f(hpos(0), hpos(1), vpos); + _pos_prev = pos; } + pos = q_error.rotateVector(pos); _h_drift += _param_fev_h_drift_rate.get() * dt; - hpos += matrix::Vector2f(_h_drift, _h_drift); - const float vpos = -((float)gps.alt * 1e-3f - _alt_ref); - matrix::Vector3f(hpos(0), hpos(1), vpos).copyTo(odom.position); + pos += matrix::Vector3f(_h_drift, _h_drift, 0.f); + pos.copyTo(odom.position); const float hvar = std::pow(gps.eph, 2.f); const float vvar = std::pow(gps.epv, 2.f); matrix::Vector3f(hvar, hvar, vvar).copyTo(odom.position_variance); - odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; } else { _pos_ref.initReference(gps.lat / 1.0e7, gps.lon / 1.0e7, gps.timestamp); @@ -138,7 +150,10 @@ vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps) } if (gps.vel_ned_valid) { - matrix::Vector3f(gps.vel_n_m_s, gps.vel_e_m_s, gps.vel_d_m_s).copyTo(odom.velocity); + matrix::Vector3f gnss_vel(gps.vel_n_m_s, gps.vel_e_m_s, gps.vel_d_m_s); + matrix::Vector3f ev_vel = q_error.rotateVector(gnss_vel); + + ev_vel.copyTo(odom.velocity); matrix::Vector3f vel_var; vel_var.setAll(std::pow(gps.s_variance_m_s, 2.f)); @@ -146,6 +161,8 @@ vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps) odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; } + (_q_att * q_error).copyTo(odom.q); + odom.timestamp = hrt_absolute_time(); return odom; diff --git a/src/modules/fake_ev/fake_ev.hpp b/src/modules/fake_ev/fake_ev.hpp index 15f7669704..dda234c84c 100644 --- a/src/modules/fake_ev/fake_ev.hpp +++ b/src/modules/fake_ev/fake_ev.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -97,11 +98,12 @@ private: vehicle_odometry_s gpsToOdom(const sensor_gps_s &gps); - uORB::Publication _vehicle_visual_odometry_pub{ORB_ID(vehicle_fake_visual_odometry)}; + uORB::Publication _vehicle_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _vehicle_attitude_sub{ORB_ID(vehicle_attitude), 1_s}; hrt_abstime _timestamp_last{0}; @@ -113,7 +115,9 @@ private: float _h_drift_rate{0.05f}; bool _is_stale{false}; - matrix::Vector2f _hpos_prev; + matrix::Vector3f _pos_prev; + + matrix::Quatf _q_att; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};