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

rename to ESC_EEPROM and fix up logic for 192 length

Browse Source
This commit is contained in:
Jacob Dahl
2025-11-28 11:07:42 -09:00
parent 22ac08547e
commit e6744590cf
17 changed files with 110 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/Am32EepromRead.msg
-5
View File
@@ -1,5 +0,0 @@
uint64 timestamp # time since system start (microseconds)
uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc.)
uint8[48] data # Raw AM32 EEPROM data
uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up reponses
msg/Am32EepromWrite.msg
-6
View File
@@ -1,6 +0,0 @@
uint64 timestamp # time since system start (microseconds)
uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc, 255 = All)
uint8[48] data # Raw AM32 EEPROM data
uint32[2] write_mask # Bitmask indicating which bytes in the data array should be written (max 64 values, am32 is currently 48)
uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up requests
msg/CMakeLists.txt
+2 -2
View File
@@ -69,8 +69,8 @@ set(msg_files
DistanceSensorModeChangeRequest.msg
DronecanNodeStatus.msg
Ekf2Timestamps.msg
Am32EepromRead.msg
Am32EepromWrite.msg
EscEepromRead.msg
EscEepromWrite.msg
EscReport.msg
EscStatus.msg
EstimatorAidSource1d.msg
msg/EscEepromRead.msg
+7
View File
@@ -0,0 +1,7 @@
uint64 timestamp # time since system start (microseconds)
uint8 firmware # ESC firmware type (see ESC_FIRMWARE enum in MAVLink)
uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc.)
uint16 length # Length of valid data
uint8[192] data # Raw ESC EEPROM data
uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up responses
msg/EscEepromWrite.msg
+8
View File
@@ -0,0 +1,8 @@
uint64 timestamp # time since system start (microseconds)
uint8 firmware # ESC firmware type (see ESC_FIRMWARE enum in MAVLink)
uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc, 255 = All)
uint16 length # Length of valid data
uint8[192] data # Raw ESC EEPROM data
uint32[6] write_mask # Bitmask indicating which bytes in the data array should be written (max 192 values)
uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up requests
msg/versioned/VehicleCommand.msg
+1 -1
View File
@@ -80,7 +80,7 @@ uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information
uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)|
uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command. |[@range -1,1] value|[s] timeout|Unused|Unused|output function|
uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command. |configuration|Unused|Unused|Unused|output function|
uint16 VEHICLE_CMD_AM32_REQUEST_EEPROM = 312 # Request EEPROM data from an AM32 ESC. |esc index|
uint16 VEHICLE_CMD_ESC_REQUEST_EEPROM = 312 # Request EEPROM data from an ESC. |esc index|firmware type (ESC_FIRMWARE enum)|
uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component. |1 to arm, 0 to disarm.
uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs a target system to run pre-arm checks.
uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes.
src/drivers/dshot/DShot.cpp
+27 -27
View File
@@ -143,7 +143,7 @@ bool DShot::updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_
// Select next command to send (if any)
if (_telemetry.telemetryResponseFinished() &&
_current_command.finished() && _telemetry.commandResponseFinished()) {
_current_command.finished() && _telemetry.commandResponseFinished()) {
select_next_command();
}
@@ -173,18 +173,18 @@ void DShot::select_next_command()
if (!_dshot_programming_active) {
// Get the next update from the queue?
if (_am32_eeprom_write_sub.updated()) {
if (_esc_eeprom_write_sub.updated()) {
// TODO: because uORB can't queue?? (what is the point of the ORB_QUEUE_LENGTH then??)
auto last = _am32_eeprom_write_sub.get_last_generation();
_am32_eeprom_write_sub.copy(&_am32_eeprom_write);
auto current = _am32_eeprom_write_sub.get_last_generation();
auto last = _esc_eeprom_write_sub.get_last_generation();
_esc_eeprom_write_sub.copy(&_esc_eeprom_write);
auto current = _esc_eeprom_write_sub.get_last_generation();
if (current != last + 1) {
PX4_ERR("am32_eeprom_write lost, generation %u -> %u", last, current);
PX4_ERR("esc_eeprom_write lost, generation %u -> %u", last, current);
}
PX4_INFO("ESC%u: starting programming mode", _am32_eeprom_write.index + 1);
PX4_INFO("ESC%u: starting programming mode", _esc_eeprom_write.index + 1);
_dshot_programming_active = true;
}
}
@@ -248,11 +248,11 @@ void DShot::select_next_command()
int next_index = -1;
// Find settings that need to be written but haven't been yet
for (int i = 0; i < 48; i++) {
for (int i = 0; i < _esc_eeprom_write.length; i++) {
int array_index = i / 32;
int bit_index = i % 32;
bool needs_write = _am32_eeprom_write.write_mask[array_index] & (1 << bit_index);
bool needs_write = _esc_eeprom_write.write_mask[array_index] & (1 << bit_index);
bool already_written = _settings_written_mask[array_index] & (1 << bit_index);
if (needs_write && !already_written) {
@@ -263,18 +263,18 @@ void DShot::select_next_command()
if (next_index >= 0) {
// Set up the motor mask based on the index in the write request
if (_am32_eeprom_write.index == 255) {
if (_esc_eeprom_write.index == 255) {
// _current_command.motor_mask = 0xFF; // Apply to all ESCs
PX4_INFO("ESC ALL: Writing setting at index %d, value %u", next_index, _am32_eeprom_write.data[next_index]);
PX4_INFO("ESC ALL: Writing setting at index %d, value %u", next_index, _esc_eeprom_write.data[next_index]);
} else {
// _current_command.motor_mask = (1 << _am32_eeprom_write.index);
PX4_INFO("ESC%d: Writing setting at index %d, value %u", _am32_eeprom_write.index + 1, next_index,
_am32_eeprom_write.data[next_index]);
// _current_command.motor_mask = (1 << _esc_eeprom_write.index);
PX4_INFO("ESC%d: Writing setting at index %d, value %u", _esc_eeprom_write.index + 1, next_index,
_esc_eeprom_write.data[next_index]);
}
_programming_address = next_index;
_programming_value = _am32_eeprom_write.data[next_index];
_programming_value = _esc_eeprom_write.data[next_index];
_programming_state = ProgrammingState::EnterMode;
// Pre-emptively Mark this setting as written
@@ -290,7 +290,7 @@ void DShot::select_next_command()
// _programming_state = ProgrammingState::Save;
_current_command.command = DSHOT_CMD_SAVE_SETTINGS;
_current_command.num_repetitions = 6;
_current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index);
_current_command.motor_mask = _esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index);
_programming_state = ProgrammingState::Idle;
// Clear the written mask for this motor for next time
@@ -298,8 +298,8 @@ void DShot::select_next_command()
_settings_written_mask[1] = 0;
// Mark as offline and unread so that we read again
// _serial_telem_online_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index));
_settings_requested_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index));
// _serial_telem_online_mask &= ~(_esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index));
_settings_requested_mask &= ~(_esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index));
_serial_telem_delay_until = hrt_absolute_time() + 500_ms;
}
@@ -309,28 +309,28 @@ void DShot::select_next_command()
case ProgrammingState::EnterMode:
_current_command.command = DSHOT_CMD_ENTER_PROGRAMMING_MODE;
_current_command.num_repetitions = 6;
_current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index);
_current_command.motor_mask = _esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index);
_programming_state = ProgrammingState::SendAddress;
break;
case ProgrammingState::SendAddress:
_current_command.command = _programming_address;
_current_command.num_repetitions = 1;
_current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index);
_current_command.motor_mask = _esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index);
_programming_state = ProgrammingState::SendValue;
break;
case ProgrammingState::SendValue:
_current_command.command = _programming_value;
_current_command.num_repetitions = 1;
_current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index);
_current_command.motor_mask = _esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index);
_programming_state = ProgrammingState::ExitMode;
break;
case ProgrammingState::ExitMode:
_current_command.command = DSHOT_CMD_EXIT_PROGRAMMING_MODE;
_current_command.num_repetitions = 1;
_current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index);
_current_command.motor_mask = _esc_eeprom_write.index == 255 ? 255 : (1 << _esc_eeprom_write.index);
_programming_state = ProgrammingState::Idle;
break;
@@ -723,13 +723,13 @@ void DShot::handle_vehicle_commands()
handle_configure_actuator(command);
break;
case vehicle_command_s::VEHICLE_CMD_AM32_REQUEST_EEPROM:
handle_am32_request_eeprom(command);
case vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM:
handle_esc_request_eeprom(command);
break;
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
PX4_INFO("VEHICLE_CMD_REQUEST_MESSAGE: param1: %f", (double)command.param1);
handle_am32_request_eeprom(command);
handle_esc_request_eeprom(command);
default:
break;
@@ -808,9 +808,9 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command)
_command_ack_pub.publish(command_ack);
}
void DShot::handle_am32_request_eeprom(const vehicle_command_s &command)
void DShot::handle_esc_request_eeprom(const vehicle_command_s &command)
{
PX4_INFO("Received AM32_REQUEST_EEPROM");
PX4_INFO("Received ESC_REQUEST_EEPROM");
PX4_INFO("esc_index: %d", (int)command.param2);
int esc_index = command.param2;
src/drivers/dshot/DShot.h
+4 -4
View File
@@ -38,7 +38,7 @@
#include <px4_platform_common/module.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/am32_eeprom_write.h>
#include <uORB/topics/esc_eeprom_write.h>
#include "DShotCommon.h"
#include "DShotTelemetry.h"
@@ -119,7 +119,7 @@ private:
// Mavlink command handlers
void handle_vehicle_commands();
void handle_configure_actuator(const vehicle_command_s &command);
void handle_am32_request_eeprom(const vehicle_command_s &command);
void handle_esc_request_eeprom(const vehicle_command_s &command);
// Mixer
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
@@ -128,7 +128,7 @@ private:
// uORB
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _am32_eeprom_write_sub{ORB_ID(am32_eeprom_write)};
uORB::Subscription _esc_eeprom_write_sub{ORB_ID(esc_eeprom_write)};
uORB::PublicationMultiData<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -200,7 +200,7 @@ private:
ExitMode
};
am32_eeprom_write_s _am32_eeprom_write{};
esc_eeprom_write_s _esc_eeprom_write{};
bool _dshot_programming_active = {};
uint32_t _settings_written_mask[2] = {};
src/drivers/dshot/DShotTelemetry.h
+1 -2
View File
@@ -62,8 +62,7 @@ public:
void publish_esc_settings();
private:
static constexpr int COMMAND_RESPONSE_MAX_SIZE = 128;
static constexpr int COMMAND_RESPONSE_SETTINGS_SIZE = 49; // 48B for EEPROM + 1B for CRC
static constexpr int COMMAND_RESPONSE_MAX_SIZE = 192;
static constexpr int TELEMETRY_FRAME_SIZE = 10;
TelemetryStatus decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data);
src/drivers/dshot/esc/AM32Settings.cpp
+6 -4
View File
@@ -38,7 +38,7 @@
static constexpr int EEPROM_SIZE = 48; // AM32 sends raw eeprom data
static constexpr int RESPONSE_SIZE = 49; // 48B data + 1B CRC
uORB::Publication<am32_eeprom_read_s> AM32Settings::_am32_eeprom_read_pub{ORB_ID(am32_eeprom_read)};
uORB::Publication<esc_eeprom_read_s> AM32Settings::_esc_eeprom_read_pub{ORB_ID(esc_eeprom_read)};
AM32Settings::AM32Settings(int index)
: _esc_index(index)
@@ -52,11 +52,13 @@ int AM32Settings::getExpectedResponseSize()
void AM32Settings::publish_latest()
{
// PX4_INFO("publish_latest()");
am32_eeprom_read_s data = {};
esc_eeprom_read_s data = {};
data.timestamp = hrt_absolute_time();
data.firmware = 1; // ESC_FIRMWARE_AM32
data.index = _esc_index;
memcpy(data.data, &_eeprom_data, sizeof(data.data));
_am32_eeprom_read_pub.publish(data);
memcpy(data.data, &_eeprom_data, sizeof(_eeprom_data));
data.length = sizeof(_eeprom_data);
_esc_eeprom_read_pub.publish(data);
}
bool AM32Settings::decodeInfoResponse(const uint8_t *buf, int size)
src/drivers/dshot/esc/AM32Settings.h
+2 -2
View File
@@ -35,7 +35,7 @@
#include "ESCSettingsInterface.h"
#include <uORB/Publication.hpp>
#include <uORB/topics/am32_eeprom_read.h>
#include <uORB/topics/esc_eeprom_read.h>
class AM32Settings : public ESCSettingsInterface
{
@@ -99,5 +99,5 @@ private:
int _esc_index{};
EEPROMData _eeprom_data{};
static uORB::Publication<am32_eeprom_read_s> _am32_eeprom_read_pub;
static uORB::Publication<esc_eeprom_read_s> _esc_eeprom_read_pub;
};
src/modules/logger/logged_topics.cpp
+2 -2
View File
@@ -38,7 +38,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <uORB/topics/uORBTopics.hpp>
#include <uORB/topics/am32_eeprom_read.h> // TODO: debugging
#include <uORB/topics/esc_eeprom_read.h> // TODO: debugging
#include <string.h>
@@ -46,7 +46,7 @@ using namespace px4::logger;
void LoggedTopics::add_default_topics()
{
add_topic("am32_eeprom_read"); // TODO: debugging
add_topic("esc_eeprom_read"); // TODO: debugging
add_topic("action_request");
add_topic("actuator_armed");
add_optional_topic("actuator_controls_status_0", 300);
src/modules/mavlink/mavlink_main.cpp
+5 -5
View File
@@ -1431,7 +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("ESC_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);
@@ -1498,7 +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("ESC_EEPROM", unlimited_rate);
configure_stream_local("MOUNT_ORIENTATION", 10.0f);
configure_stream_local("OBSTACLE_DISTANCE", 10.0f);
configure_stream_local("ODOMETRY", 30.0f);
@@ -1677,7 +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("ESC_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);
@@ -1773,7 +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("ESC_EEPROM", unlimited_rate);
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
@@ -1842,7 +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("ESC_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
+5 -5
View File
@@ -137,8 +137,8 @@
#include "streams/CURRENT_MODE.hpp"
#endif
#ifdef MAVLINK_MSG_ID_AM32_EEPROM // Only defined if development.xml is used
#include "streams/AM32_EEPROM.hpp"
#ifdef MAVLINK_MSG_ID_ESC_EEPROM // Only defined if development.xml is used
#include "streams/ESC_EEPROM.hpp"
#endif
#if !defined(CONSTRAINED_FLASH)
@@ -472,9 +472,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(ESC_EEPROM_HPP)
create_stream_list_item<MavlinkStreamEscEeprom>(),
#endif // ESC_EEPROM_HPP
#if defined(AUTOPILOT_VERSION_HPP)
create_stream_list_item<MavlinkStreamAutopilotVersion>(),
#endif // AUTOPILOT_VERSION_HPP
src/modules/mavlink/mavlink_receiver.cpp
+17 -17
View File
@@ -332,10 +332,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
#endif
#if defined(MAVLINK_MSG_ID_AM32_EEPROM) // For now only defined if development.xml is used
#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
case MAVLINK_MSG_ID_AM32_EEPROM:
handle_message_am32_eeprom(msg);
case MAVLINK_MSG_ID_ESC_EEPROM:
handle_message_esc_eeprom(msg);
break;
#endif
@@ -602,8 +602,9 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
if (message_id == MAVLINK_MSG_ID_AM32_EEPROM) {
PX4_INFO("publishing MAV_CMD_REQUEST_MESSAGE for MAVLINK_MSG_ID_AM32_EEPROM");
// NOTE: ESC_EEPROM message request handling is deferred - DShot driver handles and triggers reading
if (message_id == MAVLINK_MSG_ID_ESC_EEPROM) {
PX4_INFO("publishing MAV_CMD_REQUEST_MESSAGE for MAVLINK_MSG_ID_ESC_EEPROM");
_cmd_pub.publish(vehicle_command);
} else if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
@@ -1316,15 +1317,16 @@ 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
#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
void
MavlinkReceiver::handle_message_am32_eeprom(mavlink_message_t *msg)
MavlinkReceiver::handle_message_esc_eeprom(mavlink_message_t *msg)
{
mavlink_am32_eeprom_t message;
mavlink_msg_am32_eeprom_decode(msg, &message);
mavlink_esc_eeprom_t message;
mavlink_msg_esc_eeprom_decode(msg, &message);
am32_eeprom_write_s eeprom{};
esc_eeprom_write_s eeprom{};
eeprom.timestamp = hrt_absolute_time();
eeprom.firmware = message.firmware;
eeprom.index = message.esc_index;
uint8_t min_length = sizeof(eeprom.data);
@@ -1343,16 +1345,14 @@ MavlinkReceiver::handle_message_am32_eeprom(mavlink_message_t *msg)
}
}
// 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];
eeprom.length = length;
memcpy(eeprom.write_mask, message.write_mask, sizeof(eeprom.write_mask));
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]);
PX4_INFO("ESC EEPROM write request for ESC%d firmware %d", eeprom.index + 1, eeprom.firmware);
_am32_eeprom_write_pub.publish(eeprom);
_esc_eeprom_write_pub.publish(eeprom);
}
#endif // MAVLINK_MSG_ID_AM32_EEPROM
#endif // MAVLINK_MSG_ID_ESC_EEPROM
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
src/modules/mavlink/mavlink_receiver.h
+4 -4
View File
@@ -64,7 +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/esc_eeprom_write.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_status.h>
#include <uORB/topics/cellular_status.h>
@@ -204,8 +204,8 @@ private:
#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);
#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
void handle_message_esc_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);
@@ -332,7 +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)};
uORB::Publication<esc_eeprom_write_s> _esc_eeprom_write_pub{ORB_ID(esc_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 → src/modules/mavlink/streams/ESC_EEPROM.hpp
+19 -19
View File
@@ -31,50 +31,50 @@
*
****************************************************************************/
#ifndef AM32_EEPROM_HPP
#define AM32_EEPROM_HPP
#ifndef ESC_EEPROM_HPP
#define ESC_EEPROM_HPP
#include <uORB/topics/am32_eeprom_read.h>
#include <uORB/topics/esc_eeprom_read.h>
class MavlinkStreamAM32Eeprom : public MavlinkStream
class MavlinkStreamEscEeprom : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAM32Eeprom(mavlink); }
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEscEeprom(mavlink); }
static constexpr const char *get_name_static() { return "AM32_EEPROM"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AM32_EEPROM; }
static constexpr const char *get_name_static() { return "ESC_EEPROM"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESC_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;
return _esc_eeprom_read_sub.advertised() ? MAVLINK_MSG_ID_ESC_EEPROM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamAM32Eeprom(Mavlink *mavlink) : MavlinkStream(mavlink) {}
explicit MavlinkStreamEscEeprom(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _am32_eeprom_read_sub{ORB_ID(am32_eeprom_read)};
uORB::Subscription _esc_eeprom_read_sub{ORB_ID(esc_eeprom_read)};
bool emit_message(bool force)
{
am32_eeprom_read_s eeprom = {};
esc_eeprom_read_s eeprom = {};
if (_am32_eeprom_read_sub.update(&eeprom) || force) {
mavlink_am32_eeprom_t msg = {};
if (_esc_eeprom_read_sub.update(&eeprom) || force) {
mavlink_esc_eeprom_t msg = {};
msg.firmware = eeprom.firmware;
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);
msg.length = eeprom.length;
PX4_INFO("Sending AM32_EEPROM on channel %d", _mavlink->get_channel());
PX4_INFO("ESC%d", msg.esc_index + 1);
PX4_INFO("index %d", msg.esc_index);
PX4_INFO("Sending ESC_EEPROM on channel %d", _mavlink->get_channel());
PX4_INFO("ESC%d firmware %d", msg.esc_index + 1, msg.firmware);
PX4_INFO("length %d", msg.length);
mavlink_msg_am32_eeprom_send_struct(_mavlink->get_channel(), &msg);
mavlink_msg_esc_eeprom_send_struct(_mavlink->get_channel(), &msg);
return true;
}
@@ -94,4 +94,4 @@ private:
};
#endif // AM32_EEPROM_HPP
#endif // ESC_EEPROM_HPP