From 2f8cb5a1c2e2458c8403fc7ef93f6d61f86d66fc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 18 Oct 2021 12:17:38 -0400 Subject: [PATCH] mavlink: initial support for MAG_CAL_REPORT stream --- src/lib/sensor_calibration/Magnetometer.hpp | 2 + src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 4 + .../mavlink/streams/MAG_CAL_REPORT.hpp | 114 ++++++++++++++++++ 4 files changed, 121 insertions(+) create mode 100644 src/modules/mavlink/streams/MAG_CAL_REPORT.hpp diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index b4781675dc..663416aaff 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -69,7 +69,9 @@ public: void set_rotation(Rotation rotation); void set_temperature(float temperature) { _temperature = temperature; }; + bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } + int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } bool external() const { return _external; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 99575b24c8..e69b05fc53 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1749,6 +1749,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HIGHRES_IMU", 50.0f); configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("MAG_CAL_REPORT", 1.0f); configure_stream_local("MANUAL_CONTROL", 5.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0faae2f72b..e529033f77 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -87,6 +87,7 @@ #include "streams/HOME_POSITION.hpp" #include "streams/LANDING_TARGET.hpp" #include "streams/LOCAL_POSITION_NED.hpp" +#include "streams/MAG_CAL_REPORT.hpp" #include "streams/MANUAL_CONTROL.hpp" #include "streams/MOUNT_ORIENTATION.hpp" #include "streams/NAV_CONTROLLER_OUTPUT.hpp" @@ -385,6 +386,9 @@ static const StreamListItem streams_list[] = { #if defined(LOCAL_POSITION_NED_HPP) create_stream_list_item(), #endif // LOCAL_POSITION_NED_HPP +#if defined(MAG_CAL_REPORT_HPP) + create_stream_list_item(), +#endif // MAG_CAL_REPORT_HPP #if defined(ODOMETRY_HPP) create_stream_list_item(), #endif // ODOMETRY_HPP diff --git a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp new file mode 100644 index 0000000000..da29401ece --- /dev/null +++ b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef MAG_CAL_REPORT_HPP +#define MAG_CAL_REPORT_HPP + +#include + +#include +#include + +class MavlinkStreamMagCalReport : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMagCalReport(mavlink); } + + static constexpr const char *get_name_static() { return "MAG_CAL_REPORT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MAG_CAL_REPORT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _parameter_update_sub.advertised() ? MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamMagCalReport(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::SubscriptionMultiArray _sensor_mag_subs{ORB_ID::sensor_mag}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + bool send() override + { + bool sent = false; + parameter_update_s parameter_update; + + if (_parameter_update_sub.update(¶meter_update)) { + for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { + sensor_mag_s sensor_mag; + + if (_sensor_mag_subs[mag].update(&sensor_mag) && (sensor_mag.device_id != 0)) { + calibration::Magnetometer calibration{sensor_mag.device_id, sensor_mag.is_external}; + + if (calibration.calibrated()) { + mavlink_mag_cal_report_t msg{}; + + msg.compass_id = mag; + msg.cal_mask = 0; // TODO: Bitmask of compasses being calibrated. + msg.cal_status = MAG_CAL_SUCCESS; // TODO: Calibration Status. + msg.fitness = 0; // TODO: RMS milligauss residuals. + + msg.ofs_x = calibration.offset()(0); + msg.ofs_y = calibration.offset()(1); + msg.ofs_z = calibration.offset()(2); + + msg.diag_x = calibration.scale()(0, 0); + msg.diag_y = calibration.scale()(1, 1); + msg.diag_z = calibration.scale()(2, 2); + + msg.offdiag_x = calibration.scale()(0, 1); + msg.offdiag_y = calibration.scale()(0, 2); + msg.offdiag_z = calibration.scale()(1, 2); + + msg.orientation_confidence = 1.f; // TODO: orientation_confidence + msg.old_orientation = calibration.rotation_enum(); // TODO: old orientation + msg.new_orientation = calibration.rotation_enum(); + msg.scale_factor = 1.f; + + mavlink_msg_mag_cal_report_send_struct(_mavlink->get_channel(), &msg); + sent = true; + } + } + } + } + + return sent; + } +}; + +#endif // MAG_CAL_REPORT_HPP