From 050eedc4f8b65a2273683e658eddebcf92009e7d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 27 Jun 2016 10:16:24 -0400 Subject: [PATCH] mavlink publish WIND_COV (#4913) * mavlink publish WIND_COV -closes #4678 * px4fmu-v2_default disable logger and sync configs --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 +- cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 13 ++- cmake/configs/nuttx_px4fmu-v2_test.cmake | 7 +- src/modules/mavlink/mavlink_main.cpp | 4 + src/modules/mavlink/mavlink_messages.cpp | 109 ++++++++++++++++++-- 5 files changed, 115 insertions(+), 20 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 1699e6df9d..72876e8800 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -101,7 +101,7 @@ set(config_module_list # # Logging # - modules/logger + #modules/logger modules/sdlog2 # diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 1eeab2fa96..f43b782045 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -41,11 +41,13 @@ set(config_module_list modules/sensors #drivers/mkblctrl drivers/px4flow - drivers/oreoled + #drivers/oreoled drivers/gimbal drivers/pwm_input drivers/camera_trigger drivers/bst + #drivers/snapdragon_rc_pwm + #drivers/lis3mdl # # System commands @@ -64,6 +66,9 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver + #systemcmds/sd_bench + #systemcmds/tests + systemcmds/motor_ramp # # General system control @@ -72,7 +77,7 @@ set(config_module_list modules/load_mon modules/navigator modules/mavlink - modules/gpio_led + #modules/gpio_led modules/uavcan modules/land_detector @@ -94,8 +99,8 @@ set(config_module_list # # Logging # - modules/sdlog2 modules/logger + modules/sdlog2 # # Library modules @@ -136,7 +141,7 @@ set(config_module_list # # Rover apps # - examples/rover_steering_control + #examples/rover_steering_control # # Demo apps diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake index 27bdf6ea8a..80f8649e3d 100644 --- a/cmake/configs/nuttx_px4fmu-v2_test.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake @@ -48,7 +48,6 @@ set(config_module_list #drivers/bst #drivers/snapdragon_rc_pwm #drivers/lis3mdl - #drivers/bmi160 # # System commands @@ -67,8 +66,9 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver - systemcmds/sd_bench + #systemcmds/sd_bench systemcmds/tests + systemcmds/motor_ramp # # Testing @@ -87,7 +87,7 @@ set(config_module_list modules/load_mon modules/navigator modules/mavlink - modules/gpio_led + #modules/gpio_led modules/uavcan modules/land_detector @@ -102,7 +102,6 @@ set(config_module_list # # Vehicle Control # - # modules/segway # XXX Needs GCC 4.7 fix modules/fw_pos_control_l1 modules/fw_att_control modules/mc_att_control diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1d4d3a58b3..30ad39d3d3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1868,6 +1868,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ESTIMATOR_STATUS", 0.5f); configure_stream("ADSB_VEHICLE", 2.0f); configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f); + configure_stream("WIND", 2.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1899,6 +1900,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ESTIMATOR_STATUS", 1.0f); configure_stream("ADSB_VEHICLE", 10.0f); configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream("WIND", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1915,6 +1917,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("EXTENDED_SYS_STATE", 1.0f); configure_stream("ALTITUDE", 1.0f); configure_stream("ESTIMATOR_STATUS", 1.0f); + configure_stream("WIND", 2.0f); break; case MAVLINK_MODE_MAGIC: @@ -1949,6 +1952,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ESTIMATOR_STATUS", 5.0f); configure_stream("ADSB_VEHICLE", 20.0f); configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream("WIND", 10.0f); default: break; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 125530fb98..b62f150dbc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -39,13 +39,21 @@ * @author Anton Babushkin */ -#include #include #include +#include "mavlink_main.h" +#include "mavlink_messages.h" + #include +#include +#include #include -#include +#include +#include +#include +#include + #include #include #include @@ -57,10 +65,10 @@ #include #include #include +#include #include #include #include -#include #include #include #include @@ -79,15 +87,9 @@ #include #include #include -#include -#include -#include -#include - -#include +#include +#include -#include "mavlink_messages.h" -#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, @@ -3127,6 +3129,90 @@ protected: } }; +class MavlinkStreamWind : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamWind::get_name_static(); + } + + static const char *get_name_static() + { + return "WIND"; + } + + static uint8_t get_id_static() + { + return MAVLINK_MSG_ID_WIND_COV; + } + + uint8_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamWind(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_wind_estimate_sub; + uint64_t _wind_estimate_time; + + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time; + + /* do not allow top copying this class */ + MavlinkStreamWind(MavlinkStreamWind &); + MavlinkStreamWind& operator = (const MavlinkStreamWind &); + +protected: + explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink), + _wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))), + _wind_estimate_time(0), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _global_pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct wind_estimate_s wind_estimate; + + bool updated = _wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate); + + if (updated) { + + mavlink_wind_cov_t msg; + + msg.time_usec = wind_estimate.timestamp; + + msg.wind_x = wind_estimate.windspeed_north; + msg.wind_y = wind_estimate.windspeed_east; + msg.wind_z = 0.0f; + + msg.var_horiz = wind_estimate.covariance_north + wind_estimate.covariance_east; + msg.var_vert = 0.0f; + + struct vehicle_global_position_s global_pos; + _global_pos_sub->update(&_global_pos_time, &global_pos); + msg.wind_alt = (_global_pos_time > 0) ? global_pos.alt : NAN; + + + msg.horiz_accuracy = 0.0f; + msg.vert_accuracy = 0.0f; + + mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg); + } + } +}; + const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), @@ -3169,5 +3255,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static), new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static), new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static), + new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static), nullptr };