From 1d576c074be4dc5d936b4936f65dd2b1ba0048ed Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 24 Nov 2025 13:09:55 -0900 Subject: [PATCH] added am32_eeprom to mavlink --- src/modules/mavlink/mavlink_main.cpp | 5 ++ src/modules/mavlink/mavlink_messages.cpp | 4 + src/modules/mavlink/mavlink_receiver.cpp | 51 +++++++++++ src/modules/mavlink/mavlink_receiver.h | 5 ++ src/modules/mavlink/streams/AM32_EEPROM.hpp | 96 +++++++++++++++++++++ 5 files changed, 161 insertions(+) create mode 100644 src/modules/mavlink/streams/AM32_EEPROM.hpp diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b72a220b0..ae3da91a0d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1431,6 +1431,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("EFI_STATUS", 2.0f); configure_stream_local("ESC_INFO", 1.0f); configure_stream_local("ESC_STATUS", 1.0f); + configure_stream_local("AM32_EEPROM", unlimited_rate); configure_stream_local("ESTIMATOR_STATUS", 0.5f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); @@ -1497,6 +1498,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DISTANCE_SENSOR", 10.0f); configure_stream_local("ESC_INFO", 10.0f); configure_stream_local("ESC_STATUS", 10.0f); + configure_stream_local("AM32_EEPROM", unlimited_rate); configure_stream_local("MOUNT_ORIENTATION", 10.0f); configure_stream_local("OBSTACLE_DISTANCE", 10.0f); configure_stream_local("ODOMETRY", 30.0f); @@ -1675,6 +1677,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("EFI_STATUS", 10.0f); configure_stream_local("ESC_INFO", 10.0f); configure_stream_local("ESC_STATUS", 10.0f); + configure_stream_local("AM32_EEPROM", unlimited_rate); configure_stream_local("ESTIMATOR_STATUS", 5.0f); configure_stream_local("EXTENDED_SYS_STATE", 2.0f); configure_stream_local("GLOBAL_POSITION_INT", 10.0f); @@ -1770,6 +1773,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); configure_stream_local("ESC_INFO", 1.0f); configure_stream_local("ESC_STATUS", 5.0f); + configure_stream_local("AM32_EEPROM", unlimited_rate); configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ATTITUDE_TARGET", 2.0f); @@ -1838,6 +1842,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f); configure_stream_local("ESC_INFO", 1.0f); configure_stream_local("ESC_STATUS", 2.0f); + configure_stream_local("AM32_EEPROM", unlimited_rate); configure_stream_local("ADSB_VEHICLE", 1.0f); configure_stream_local("ATTITUDE_TARGET", 0.5f); configure_stream_local("AVAILABLE_MODES", 0.3f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a68c138747..3605262236 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -62,6 +62,7 @@ #include "streams/ATTITUDE_QUATERNION.hpp" #include "streams/ATTITUDE_TARGET.hpp" #include "streams/AUTOPILOT_VERSION.hpp" +#include "streams/AM32_EEPROM.hpp" #include "streams/BATTERY_STATUS.hpp" #include "streams/CAMERA_IMAGE_CAPTURED.hpp" #include "streams/CAMERA_TRIGGER.hpp" @@ -468,6 +469,9 @@ static const StreamListItem streams_list[] = { #if defined(ESC_STATUS_HPP) create_stream_list_item(), #endif // ESC_STATUS_HPP +#if defined(AM32_EEPROM_HPP) + create_stream_list_item(), +#endif // AM32_EEPROM_HPP #if defined(AUTOPILOT_VERSION_HPP) create_stream_list_item(), #endif // AUTOPILOT_VERSION_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6143c0a0dd..95e401c80e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -329,6 +329,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_SET_VELOCITY_LIMITS: handle_message_set_velocity_limits(msg); break; + +#endif + +#if defined(MAVLINK_MSG_ID_AM32_EEPROM) // For now only defined if development.xml is used + + case MAVLINK_MSG_ID_AM32_EEPROM: + handle_message_am32_eeprom(msg); + break; #endif default: @@ -1304,6 +1312,49 @@ void MavlinkReceiver::handle_message_set_velocity_limits(mavlink_message_t *msg) } #endif // MAVLINK_MSG_ID_SET_VELOCITY_LIMITS +#if defined(MAVLINK_MSG_ID_AM32_EEPROM) // For now only defined if development.xml is used +void +MavlinkReceiver::handle_message_am32_eeprom(mavlink_message_t *msg) +{ + mavlink_am32_eeprom_t message; + mavlink_msg_am32_eeprom_decode(msg, &message); + + // Only handle write requests + if (message.mode == 0) { + return; + } + + am32_eeprom_write_s eeprom{}; + eeprom.timestamp = hrt_absolute_time(); + eeprom.index = message.index; + + uint8_t min_length = sizeof(eeprom.data); + int length = message.length; + + if (length > min_length) { + length = min_length; + } + + for (int i = 0; i < length && i < min_length; i++) { + int mask_index = i / 32; // Which uint32_t in the write_mask array + int bit_index = i % 32; // Which bit within that uint32_t + + if (message.write_mask[mask_index] & (1U << bit_index)) { + eeprom.data[i] = message.data[i]; + } + } + + // Copy the write mask (only first 2 uint32_t needed for 48 bytes) + eeprom.write_mask[0] = message.write_mask[0]; + eeprom.write_mask[1] = message.write_mask[1]; + + PX4_INFO("AM32 EEPROM write request for ESC%d, mask: 0x%08" PRIx32 "%08" PRIx32, + eeprom.index + 1, eeprom.write_mask[1], eeprom.write_mask[0]); + + _am32_eeprom_write_pub.publish(eeprom); +} +#endif // MAVLINK_MSG_ID_AM32_EEPROM + void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d1c477e1c7..9912ef3255 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -202,6 +203,9 @@ private: void handle_message_utm_global_position(mavlink_message_t *msg); #if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used void handle_message_set_velocity_limits(mavlink_message_t *msg); +#endif +#if defined(MAVLINK_MSG_ID_AM32_EEPROM) // For now only defined if development.xml is used + void handle_message_am32_eeprom(mavlink_message_t *msg); #endif void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg); @@ -328,6 +332,7 @@ private: uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _am32_eeprom_write_pub{ORB_ID(am32_eeprom_write)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; diff --git a/src/modules/mavlink/streams/AM32_EEPROM.hpp b/src/modules/mavlink/streams/AM32_EEPROM.hpp new file mode 100644 index 0000000000..21006e5b73 --- /dev/null +++ b/src/modules/mavlink/streams/AM32_EEPROM.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 AM32_EEPROM_HPP +#define AM32_EEPROM_HPP + +#include + +class MavlinkStreamAM32Eeprom : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAM32Eeprom(mavlink); } + + static constexpr const char *get_name_static() { return "AM32_EEPROM"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AM32_EEPROM; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _am32_eeprom_read_sub.advertised() ? MAVLINK_MSG_ID_AM32_EEPROM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamAM32Eeprom(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _am32_eeprom_read_sub{ORB_ID(am32_eeprom_read)}; + + bool request_message(float param2, float param3, float param4, float param5, float param6, float param7) override + { + emit_message(true) + } + + bool send() override + { + emit_message(false); + } + + bool emit_message(bool force) + { + am32_eeprom_read_s eeprom = {}; + + if (_am32_eeprom_read_sub.update(&eeprom) || force) { + mavlink_am32_eeprom_t msg = {}; + msg.esc_index = eeprom.index; + msg.msg_index = 0; + msg.msg_count = 1; + memcpy(msg.data, eeprom.data, sizeof(eeprom.data)); + msg.length = sizeof(eeprom.data); + + PX4_INFO("Sending AM32_EEPROM on channel %d", _mavlink->get_channel()); + PX4_INFO("ESC%d", msg.index + 1); + PX4_INFO("index %d", msg.index); + PX4_INFO("length %d", msg.length); + + mavlink_msg_am32_eeprom_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // AM32_EEPROM_HPP