fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-11 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added am32_eeprom to mavlink

Browse Source
This commit is contained in:
Jacob Dahl
2026-01-27 15:39:49 -09:00
parent 9761cb3d6f
commit 1d576c074b
5 changed files with 161 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+5
View File
@@ -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);
src/modules/mavlink/mavlink_messages.cpp
+4
View File
@@ -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<MavlinkStreamESCStatus>(),
#endif // ESC_STATUS_HPP
#if defined(AM32_EEPROM_HPP)
create_stream_list_item<MavlinkStreamAM32Eeprom>(),
#endif // AM32_EEPROM_HPP
#if defined(AUTOPILOT_VERSION_HPP)
create_stream_list_item<MavlinkStreamAutopilotVersion>(),
#endif // AUTOPILOT_VERSION_HPP
src/modules/mavlink/mavlink_receiver.cpp
+51
View File
@@ -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)
{
src/modules/mavlink/mavlink_receiver.h
+5
View File
@@ -64,6 +64,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/am32_eeprom_write.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_status.h>
#include <uORB/topics/cellular_status.h>
@@ -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<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<am32_eeprom_write_s> _am32_eeprom_write_pub{ORB_ID(am32_eeprom_write)};
#if !defined(CONSTRAINED_FLASH)
uORB::Publication<debug_array_s> _debug_array_pub {ORB_ID(debug_array)};
src/modules/mavlink/streams/AM32_EEPROM.hpp
+96
View File
@@ -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 <uORB/topics/am32_eeprom_read.h>
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