yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

purge vehicle_vision_position alias

Browse Source
This commit is contained in:
TSC21
2018-07-12 18:46:18 +01:00
committed by Lorenz Meier
parent 8f23a073a4
commit 183a63cce9
7 changed files with 2294 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_local_position.msg
View File

@@ -63,4 +63,4 @@ float32 vz_max # maximum vertical speed - set to 0 when limiting not required
float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters)
float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters)
# TOPICS vehicle_local_position vehicle_local_position_groundtruth vehicle_vision_position
# TOPICS vehicle_local_position vehicle_local_position_groundtruth

2
src/modules/ekf2/ekf2_main.cpp
View File

@@ -1162,7 +1162,7 @@ void Ekf2::run()
if (visual_odometry_updated) {
// copy both attitude & position, we need both to fill a single ext_vision_message
vehicle_odometry_s ev_odom = {};
orb_copy(ORB_ID(vehicle_vision_position), _ev_odom_sub, &ev_odom);
orb_copy(ORB_ID(vehicle_visual_odometry), _ev_odom_sub, &ev_odom);
ext_vision_message ev_data;
ev_data.posNED(0) = ev_odom.x;

39
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -91,6 +91,7 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
@@ -2059,6 +2060,7 @@ protected:
}
};
//TODO: remove this -> add ODOMETRY loopback only
class MavlinkStreamVisionPositionEstimate : public MavlinkStream
{
public:
@@ -2089,15 +2091,12 @@ public:
unsigned get_size()
{
return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
return (_odom_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
MavlinkOrbSubscription *_pos_sub;
uint64_t _pos_time;
MavlinkOrbSubscription *_att_sub;
uint64_t _att_time;
MavlinkOrbSubscription *_odom_sub;
uint64_t _odom_time;
/* do not allow top copying this class */
MavlinkStreamVisionPositionEstimate(MavlinkStreamVisionPositionEstimate &) = delete;
@@ -2105,36 +2104,32 @@ private:
protected:
explicit MavlinkStreamVisionPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vision_position))),
_pos_time(0),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vision_attitude))),
_att_time(0)
_odom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_visual_odometry))),
_odom_time(0)
{}
bool send(const hrt_abstime t)
{
vehicle_local_position_s vpos = {};
vehicle_attitude_s vatt = {};
vehicle_odometry_s vodom = {};
bool att_updated = _att_sub->update(&_att_time, &vatt);
bool pos_updated = _pos_sub->update(&_pos_time, &vpos);
if (pos_updated || att_updated) {
if (_odom_sub->update(&_odom_time, &vodom)) {
mavlink_vision_position_estimate_t vmsg = {};
vmsg.usec = vpos.timestamp;
vmsg.x = vpos.x;
vmsg.y = vpos.y;
vmsg.z = vpos.z;
vmsg.usec = vodom.timestamp;
vmsg.x = vodom.x;
vmsg.y = vodom.y;
vmsg.z = vodom.z;
matrix::Eulerf euler = matrix::Quatf(vatt.q);
matrix::Eulerf euler = matrix::Quatf(vodom.q);
vmsg.roll = euler.phi();
vmsg.pitch = euler.theta();
vmsg.yaw = euler.psi();
mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
return true;
}
return (pos_updated || att_updated);
return false;
}
};

7
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@@ -59,6 +59,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/optical_flow.h>
@@ -381,7 +382,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vision_position_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
int visual_odometry_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
int vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data));
@@ -774,10 +775,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* check no vision circuit breaker is set */
if (params.no_vision != CBRK_NO_VISION_KEY) {
/* vehicle vision position */
orb_check(vision_position_sub, &updated);
orb_check(visual_odometry_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_vision_position), vision_position_sub, &vision);
orb_copy(ORB_ID(vehicle_visual_odometry), visual_odometry_sub, &vision);
static float last_vision_x = 0.0f;
static float last_vision_y = 0.0f;

2261
src/modules/sdlog2/sdlog2.c Normal file
View File

File diff suppressed because it is too large Load Diff

6
src/modules/simulator/simulator.h
View File

@@ -244,8 +244,7 @@ private:
_gyro_pub(nullptr),
_mag_pub(nullptr),
_flow_pub(nullptr),
_vision_position_pub(nullptr),
_vision_attitude_pub(nullptr),
_visual_odometry_pub(nullptr),
_dist_pub(nullptr),
_battery_pub(nullptr),
_param_sub(-1),
@@ -323,8 +322,7 @@ private:
orb_advert_t _gyro_pub;
orb_advert_t _mag_pub;
orb_advert_t _flow_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _vision_attitude_pub;
orb_advert_t _visual_odometry_pub;
orb_advert_t _dist_pub;
orb_advert_t _battery_pub;
orb_advert_t _irlock_report_pub;

25
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -46,6 +46,7 @@
#include <conversion/rotation.h>
#include <mathlib/mathlib.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <limits>
@@ -1128,28 +1129,18 @@ int Simulator::publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_local_position_s vision_position = {};
struct vehicle_odometry_s visual_odometry = {};
vision_position.timestamp = timestamp;
vision_position.x = ev_mavlink->x;
vision_position.y = ev_mavlink->y;
vision_position.z = ev_mavlink->z;
vision_position.xy_valid = true;
vision_position.z_valid = true;
vision_position.v_xy_valid = true;
vision_position.v_z_valid = true;
struct vehicle_attitude_s vision_attitude = {};
vision_attitude.timestamp = timestamp;
visual_odometry.timestamp = timestamp;
visual_odometry.x = ev_mavlink->x;
visual_odometry.y = ev_mavlink->y;
visual_odometry.z = ev_mavlink->z;
matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw));
q.copyTo(vision_attitude.q);
q.copyTo(visual_odometry.q);
int inst = 0;
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odometry, &inst, ORB_PRIO_HIGH);
return OK;
}