From 8ddc7a27c73475f1a8d4b5c5c98f0b3dd95e2189 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 18 Nov 2015 23:55:37 +0100 Subject: [PATCH 1/8] implemented altitude message draft --- src/modules/mavlink/mavlink_messages.cpp | 84 ++++++++++++++++++++++++ 1 file changed, 84 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index be3ab850f4..a4926aaed6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2537,6 +2537,89 @@ protected: } }; +class MavlinkStreamAltitude : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAltitude::get_name_static(); + } + + static const char *get_name_static() + { + return "ALTITUDE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_ALTITUDE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAltitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time; + + MavlinkOrbSubscription *_local_pos_sub; + uint64_t _local_pos_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; + + /* do not allow top copying this class */ + MavlinkStreamAltitude(MavlinkStreamAltitude &); + MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &); + +protected: + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _global_pos_time(0), + _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _local_pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_global_position_s global_pos; + struct vehicle_local_position_s local_pos; + struct home_position_s home; + + bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos); + updated |= _local_pos_sub->update(&_local_pos_time, &local_pos); + updated |= _home_sub->update(&_home_time, &home); + + if (updated) { + mavlink_altitude_t msg; + + msg.altitude_monotonic = global_pos.alt; + msg.altitude_amsl = global_pos.alt; + msg.altitude_local = -local_pos.z; + msg.altitude_relative = home.alt; + + if (global_pos.terrain_alt_valid) { + msg.altitude_terrain = global_pos.terrain_alt; + msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt; + } else { + msg.altitude_terrain = -1001; + msg.bottom_clearance = -1; + } + + _mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); + } + } +}; + const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), @@ -2574,5 +2657,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static), new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static), + new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static), nullptr }; From d9c332353aa24b56da96650fa0014541c8df2eb8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 18 Nov 2015 23:59:08 +0100 Subject: [PATCH 2/8] added altitude stream to configs --- src/modules/mavlink/mavlink_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b04eea1a0..3e6fc97845 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1689,6 +1689,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("ALTITUDE", 1.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1715,6 +1716,7 @@ Mavlink::task_main(int argc, char *argv[]) //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("ALTITUDE", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1730,6 +1732,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("EXTENDED_SYS_STATE", 1.0f); + configure_stream("ALTITUDE", 1.0f); break; case MAVLINK_MODE_MAGIC: From f5a844d4aa6c732c7aed3c5ffbc54062ef89c9c2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 20 Nov 2015 21:01:35 +0100 Subject: [PATCH 3/8] added field for pressure altitude to global position --- msg/vehicle_global_position.msg | 1 + src/modules/mavlink/mavlink_messages.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 6a38e2c9d7..cee381e871 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -18,3 +18,4 @@ float32 epv # Standard deviation of position vertically float32 terrain_alt # Terrain altitude in m, WGS84 bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning +float32 pressure_alt # Pressure altitude diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4926aaed6..b6653a83e0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2602,7 +2602,7 @@ protected: if (updated) { mavlink_altitude_t msg; - msg.altitude_monotonic = global_pos.alt; + msg.altitude_monotonic = global_pos.pressure_alt; msg.altitude_amsl = global_pos.alt; msg.altitude_local = -local_pos.z; msg.altitude_relative = home.alt; From 11d9e3a325bfdfebe4469b7b86a76822d35412f2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 20 Nov 2015 21:47:12 +0100 Subject: [PATCH 4/8] also adding message to usb config --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3e6fc97845..39b8a5c730 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1763,6 +1763,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 20.0f); configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("ALTITUDE", 10.0f); default: break; From c706c30e83735af6bfcbcd7ac4bee5bcc34df1cb Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 16:17:07 +0100 Subject: [PATCH 5/8] fill pressure altitude in global position from estimators --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 3 +++ .../local_position_estimator/BlockLocalPositionEstimator.cpp | 1 + .../position_estimator_inav/position_estimator_inav_main.cpp | 2 ++ 3 files changed, 6 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 24a198f0a5..23bc784daf 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1015,6 +1015,9 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.terrain_alt_valid = false; } + /* baro altitude */ + _global_pos.pressure_alt = _baro_alt_filt; + _global_pos.yaw = _local_pos.yaw; const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index d1262f9177..8b457a68db 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -711,6 +711,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().terrain_alt = 0; _pub_gpos.get().terrain_alt_valid = false; _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; _pub_gpos.update(); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 72b250ebe9..90bf30c606 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1336,6 +1336,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.terrain_alt_valid = false; } + global_pos.pressure_alt = sensor.baro_alt_meter[0]; + if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); From 2ceaab138f5de756fd5a993f9c2a3e1e5ea87139 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 21:19:24 +0100 Subject: [PATCH 6/8] added timestamp for altitude message --- src/modules/mavlink/mavlink_messages.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b6653a83e0..97fb3a81bf 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2602,6 +2602,8 @@ protected: if (updated) { mavlink_altitude_t msg; + msg.time_usec = hrt_absolute_time(); + msg.altitude_monotonic = global_pos.pressure_alt; msg.altitude_amsl = global_pos.alt; msg.altitude_local = -local_pos.z; From 82cf8d6ecd722747a7997355ba89c1932606b918 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:30:55 +0100 Subject: [PATCH 7/8] switch to latest mavlink --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 86a2b147e4..db5cf66d1a 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 86a2b147e4cec5066efdc6d030d69d902929e66f +Subproject commit db5cf66d1a7f549ccbaa92b5476e78754e008297 From 59f80581aa4be158a956d60ea71d45f5bb78a558 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:35:03 +0100 Subject: [PATCH 8/8] switch to _ekf->baroHgt for pressure alt in ekf --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 23bc784daf..9727f21c06 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1016,7 +1016,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* baro altitude */ - _global_pos.pressure_alt = _baro_alt_filt; + _global_pos.pressure_alt = _ekf->baroHgt; _global_pos.yaw = _local_pos.yaw;