fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-10 05:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

add fake ev attitude error

Browse Source
This commit is contained in:
bresch
2023-06-29 16:37:30 +02:00
parent 5bb811f8f9
commit 1c49812b17
2 changed files with 31 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fake_ev/fake_ev.cpp
+25 -8
View File
@@ -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;
src/modules/fake_ev/fake_ev.hpp
+6 -2
View File
@@ -50,6 +50,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_odometry.h>
using namespace time_literals;
@@ -97,11 +98,12 @@ private:
vehicle_odometry_s gpsToOdom(const sensor_gps_s &gps);
uORB::Publication<vehicle_odometry_s> _vehicle_visual_odometry_pub{ORB_ID(vehicle_fake_visual_odometry)};
uORB::Publication<vehicle_odometry_s> _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")};