diff --git a/msg/Am32EepromRead.msg b/msg/Am32EepromRead.msg deleted file mode 100644 index 786cdb541e..0000000000 --- a/msg/Am32EepromRead.msg +++ /dev/null @@ -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 diff --git a/msg/Am32EepromWrite.msg b/msg/Am32EepromWrite.msg deleted file mode 100644 index c1515f73d9..0000000000 --- a/msg/Am32EepromWrite.msg +++ /dev/null @@ -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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a8de0e829e..f813018210 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/EscEepromRead.msg b/msg/EscEepromRead.msg new file mode 100644 index 0000000000..05e0607f9c --- /dev/null +++ b/msg/EscEepromRead.msg @@ -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 diff --git a/msg/EscEepromWrite.msg b/msg/EscEepromWrite.msg new file mode 100644 index 0000000000..d9e1c63628 --- /dev/null +++ b/msg/EscEepromWrite.msg @@ -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 diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 9e7ebc08d3..9910294da9 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -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. diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 7b0444eecc..965ddaea62 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -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; diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 6b8efc8002..c18860f319 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -38,7 +38,7 @@ #include #include #include -#include +#include #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_pub{ORB_ID(esc_status)}; uORB::Publication _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] = {}; diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index 1b8c9df391..9603c048a8 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -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); diff --git a/src/drivers/dshot/esc/AM32Settings.cpp b/src/drivers/dshot/esc/AM32Settings.cpp index bef566bc4b..7965e78d96 100644 --- a/src/drivers/dshot/esc/AM32Settings.cpp +++ b/src/drivers/dshot/esc/AM32Settings.cpp @@ -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 AM32Settings::_am32_eeprom_read_pub{ORB_ID(am32_eeprom_read)}; +uORB::Publication 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) diff --git a/src/drivers/dshot/esc/AM32Settings.h b/src/drivers/dshot/esc/AM32Settings.h index c06940182d..fadc35fd0c 100644 --- a/src/drivers/dshot/esc/AM32Settings.h +++ b/src/drivers/dshot/esc/AM32Settings.h @@ -35,7 +35,7 @@ #include "ESCSettingsInterface.h" #include -#include +#include class AM32Settings : public ESCSettingsInterface { @@ -99,5 +99,5 @@ private: int _esc_index{}; EEPROMData _eeprom_data{}; - static uORB::Publication _am32_eeprom_read_pub; + static uORB::Publication _esc_eeprom_read_pub; }; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 212764d735..944c0987e4 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -38,7 +38,7 @@ #include #include #include -#include // TODO: debugging +#include // TODO: debugging #include @@ -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); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ae3da91a0d..b1d1a8fbcd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9701d411ae..c79dfe38e7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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(), #endif // ESC_STATUS_HPP -#if defined(AM32_EEPROM_HPP) - create_stream_list_item(), -#endif // AM32_EEPROM_HPP +#if defined(ESC_EEPROM_HPP) + create_stream_list_item(), +#endif // ESC_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 e253edc231..57ed503307 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9912ef3255..a62fb780f6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -64,7 +64,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 _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)}; + uORB::Publication _esc_eeprom_write_pub{ORB_ID(esc_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/ESC_EEPROM.hpp similarity index 72% rename from src/modules/mavlink/streams/AM32_EEPROM.hpp rename to src/modules/mavlink/streams/ESC_EEPROM.hpp index 69c1f98b48..49172684d9 100644 --- a/src/modules/mavlink/streams/AM32_EEPROM.hpp +++ b/src/modules/mavlink/streams/ESC_EEPROM.hpp @@ -31,50 +31,50 @@ * ****************************************************************************/ -#ifndef AM32_EEPROM_HPP -#define AM32_EEPROM_HPP +#ifndef ESC_EEPROM_HPP +#define ESC_EEPROM_HPP -#include +#include -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