dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1391 from PX4/vfr_fix 

mavlink: use altitude AMSL in VFR message
This commit is contained in:
Lorenz Meier 2014-12-06 17:51:10 +01:00
parent 72d5f2b245 88bb192722
commit b4a3dcb2f0
1 changed files with 2 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -810,9 +810,6 @@ private:
MavlinkOrbSubscription *_airspeed_sub; MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time; uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_combined_sub;
uint64_t _sensor_combined_time;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@ -828,9 +825,7 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0), _act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0), _airspeed_time(0)
_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_combined_time(0)
{} {}
void send(const hrt_abstime t) void send(const hrt_abstime t)
@ -840,14 +835,12 @@ protected:
struct actuator_armed_s armed; struct actuator_armed_s armed;
struct actuator_controls_s act; struct actuator_controls_s act;
struct airspeed_s airspeed; struct airspeed_s airspeed;
struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att); bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos); updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed); updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act); updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) { if (updated) {
mavlink_vfr_hud_t msg; mavlink_vfr_hud_t msg;
@ -856,7 +849,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
msg.alt = sensor_combined.baro_alt_meter; msg.alt = pos.alt;
msg.climb = -pos.vel_d; msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);