Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

mavlink GLOBAL_POSITION_INT use vehicle_local_position for altitude

Browse Source
This commit is contained in:
Daniel Agar
2018-03-25 13:21:08 -04:00
parent 6af989e8c0
commit deb5cbcc82
2 changed files with 46 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+46 -22
View File
@@ -1678,11 +1678,10 @@ public:
}
private:
MavlinkOrbSubscription *_pos_sub;
uint64_t _pos_time;
MavlinkOrbSubscription *_gpos_sub;
MavlinkOrbSubscription *_lpos_sub;
MavlinkOrbSubscription *_home_sub;
uint64_t _home_time;
MavlinkOrbSubscription *_sensor_sub;
/* do not allow top copying this class */
MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &);
@@ -1690,36 +1689,61 @@ private:
protected:
explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink),
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
_pos_time(0),
_gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
_lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
_home_time(0)
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
{}
bool send(const hrt_abstime t)
{
struct vehicle_global_position_s pos = {};
struct home_position_s home = {};
vehicle_global_position_s gpos = {};
vehicle_local_position_s lpos = {};
home_position_s home = {};
sensor_combined_s sensor = {};
bool updated = _pos_sub->update(&_pos_time, &pos);
updated |= _home_sub->update(&_home_time, &home);
bool updated = false;
updated |= _gpos_sub->update(&gpos);
updated |= _lpos_sub->update(&lpos);
updated |= _home_sub->update(&home);
updated |= _sensor_sub->update(&sensor);
if (updated) {
mavlink_global_position_int_t msg = {};
if (!home.valid_alt) {
home.alt = 0.0f;
if (lpos.z_valid && lpos.z_global) {
msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f;
} else {
// fall back to baro altitude
if (sensor.timestamp > 0) {
msg.alt = sensor.baro_alt_meter * 1000.0f;
}
}
msg.time_boot_ms = pos.timestamp / 1000;
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
msg.alt = pos.alt * 1000.0f;
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
msg.vx = pos.vel_n * 100.0f;
msg.vy = pos.vel_e * 100.0f;
msg.vz = pos.vel_d * 100.0f;
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
if (home.valid_alt) {
if (lpos.z_valid) {
msg.relative_alt = -(lpos.z - home.z) * 1000.0f;
} else {
msg.relative_alt = msg.alt - (home.alt * 1000.0f);
}
} else {
if (lpos.z_valid) {
msg.relative_alt = -lpos.z * 1000.0f;
}
}
msg.time_boot_ms = gpos.timestamp / 1000;
msg.lat = gpos.lat * 1e7;
msg.lon = gpos.lon * 1e7;
msg.vx = lpos.vx * 100.0f;
msg.vy = lpos.vy * 100.0f;
msg.vz = lpos.vz * 100.0f;
msg.hdg = _wrap_2pi(lpos.yaw) * M_RAD_TO_DEG_F * 100.0f;
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
}
src/modules/mavlink/mavlink_orb_subscription.cpp
-2
View File
@@ -82,8 +82,6 @@ MavlinkOrbSubscription::get_instance() const
bool
MavlinkOrbSubscription::update(uint64_t *time, void *data)
{
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.