fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_global_position: remove velocity fields (duplicates of local vx, vy, vz)

Browse Source 
* attitude_estimator_q: get velocity from local position (if available)
This commit is contained in:
Daniel Agar
2020-03-11 23:57:43 -04:00
committed by GitHub
parent 5d33b9e999
commit a89b69b0ea
19 changed files with 98 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+22 -13
View File
@@ -58,7 +58,8 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
@@ -109,7 +110,8 @@ private:
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
@@ -307,29 +309,36 @@ AttitudeEstimatorQ::Run()
}
}
if (_global_pos_sub.updated()) {
vehicle_global_position_s gpos;
if (_gps_sub.updated()) {
vehicle_gps_position_s gps;
if (_global_pos_sub.copy(&gpos)) {
if (_param_att_mag_decl_a.get() && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1_s) {
if (_gps_sub.copy(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
/* set magnetic declination automatically */
update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
update_mag_declination(math::radians(get_mag_declination(gps.lat, gps.lon)));
}
}
}
if (_param_att_acc_comp.get() && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f
&& _inited) {
if (_local_position_sub.updated()) {
vehicle_local_position_s lpos;
if (_local_position_sub.copy(&lpos)) {
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
/* position data is actual */
Vector3f vel(gpos.vel_n, gpos.vel_e, gpos.vel_d);
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
/* velocity updated */
if (_vel_prev_t != 0 && gpos.timestamp != _vel_prev_t) {
float vel_dt = (gpos.timestamp - _vel_prev_t) / 1e6f;
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = gpos.timestamp;
_vel_prev_t = lpos.timestamp;
_vel_prev = vel;
} else {