From a2808a991c1f683077d350b6d864ec0d3578f9ee Mon Sep 17 00:00:00 2001 From: vertiq-jack Date: Wed, 11 Mar 2026 11:08:05 -0400 Subject: [PATCH] fix(vertiq): Parameter Setting Reliability Update (#26521) Updates to the backend to allow faster and more reliable parameter configuration. --------- Co-authored-by: Jordan --- .../vertiq_io/iq-module-communication-cpp | 2 +- .../vertiq_configuration_handler.cpp | 65 +++++++++++-------- .../vertiq_configuration_handler.hpp | 10 +-- src/drivers/actuators/vertiq_io/vertiq_io.cpp | 2 +- src/drivers/actuators/vertiq_io/vertiq_io.hpp | 2 +- .../vertiq_io/vertiq_serial_interface.cpp | 2 +- .../vertiq_io/vertiq_telemetry_manager.cpp | 16 ++--- .../vertiq_io/vertiq_telemetry_manager.hpp | 2 +- 8 files changed, 55 insertions(+), 46 deletions(-) diff --git a/src/drivers/actuators/vertiq_io/iq-module-communication-cpp b/src/drivers/actuators/vertiq_io/iq-module-communication-cpp index c488af4e88..d05d55a2b7 160000 --- a/src/drivers/actuators/vertiq_io/iq-module-communication-cpp +++ b/src/drivers/actuators/vertiq_io/iq-module-communication-cpp @@ -1 +1 @@ -Subproject commit c488af4e8807de80739aa48efd2ea51614dd8195 +Subproject commit d05d55a2b706a660d46d725e1d35d59049a519ba diff --git a/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp index c80096ed2f..d001f37806 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp +++ b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.cpp @@ -35,7 +35,18 @@ VertiqConfigurationHandler::VertiqConfigurationHandler(VertiqSerialInterface *ser, VertiqClientManager *client_manager) : _serial_interface(ser), - _client_manager(client_manager) + _client_manager(client_manager), + _prop_input_parser_client(0) + +#ifdef CONFIG_USE_IFCI_CONFIGURATION + , _ifci_client(0) +#endif + +#ifdef CONFIG_USE_PULSING_CONFIGURATION + , _voltage_superposition_client(0) + , _pulsing_rectangular_input_parser_client(0) +#endif + { } @@ -43,47 +54,47 @@ void VertiqConfigurationHandler::InitConfigurationClients(uint8_t object_id) { _object_id_now = object_id; //Make sure we store the initial object ID - _prop_input_parser_client = new EscPropellerInputParserClient(object_id); - _client_manager->AddNewClient(_prop_input_parser_client); + _prop_input_parser_client.UpdateEntryIds(object_id); + _client_manager->AddNewClient(&_prop_input_parser_client); #ifdef CONFIG_USE_IFCI_CONFIGURATION - _ifci_client = new IQUartFlightControllerInterfaceClient(object_id); - _client_manager->AddNewClient(_ifci_client); + _ifci_client.UpdateEntryIds(object_id); + _client_manager->AddNewClient(&_ifci_client); #endif //CONFIG_USE_IFCI_CONFIGURATION #ifdef CONFIG_USE_PULSING_CONFIGURATION - _voltage_superposition_client = new VoltageSuperPositionClient(object_id); - _client_manager->AddNewClient(_voltage_superposition_client); + _voltage_superposition_client.UpdateEntryIds(object_id); + _client_manager->AddNewClient(&_voltage_superposition_client); - _pulsing_rectangular_input_parser_client = new PulsingRectangularInputParserClient(object_id); - _client_manager->AddNewClient(_pulsing_rectangular_input_parser_client); + _pulsing_rectangular_input_parser_client.UpdateEntryIds(object_id); + _client_manager->AddNewClient(&_pulsing_rectangular_input_parser_client); #endif //CONFIG_USE_PULSING_CONFIGURATION } void VertiqConfigurationHandler::InitClientEntryWrappers() { - AddNewClientEntry(param_find("VTQ_MAX_VELOCITY"), &(_prop_input_parser_client->velocity_max_)); - AddNewClientEntry(param_find("VTQ_MAX_VOLTS"), &(_prop_input_parser_client->volts_max_)); - AddNewClientEntry(param_find("VTQ_CONTROL_MODE"), &(_prop_input_parser_client->mode_)); - AddNewClientEntry(param_find("VTQ_MOTOR_DIR"), &(_prop_input_parser_client->sign_)); - AddNewClientEntry(param_find("VTQ_FC_DIR"), &(_prop_input_parser_client->flip_negative_)); + AddNewClientEntry(param_find("VTQ_MAX_VELOCITY"), &(_prop_input_parser_client.velocity_max_)); + AddNewClientEntry(param_find("VTQ_MAX_VOLTS"), &(_prop_input_parser_client.volts_max_)); + AddNewClientEntry(param_find("VTQ_CONTROL_MODE"), &(_prop_input_parser_client.mode_)); + AddNewClientEntry(param_find("VTQ_MOTOR_DIR"), &(_prop_input_parser_client.sign_)); + AddNewClientEntry(param_find("VTQ_FC_DIR"), &(_prop_input_parser_client.flip_negative_)); #ifdef CONFIG_USE_IFCI_CONFIGURATION - AddNewClientEntry(param_find("VTQ_THROTTLE_CVI"), &(_ifci_client->throttle_cvi_)); + AddNewClientEntry(param_find("VTQ_THROTTLE_CVI"), &(_ifci_client.throttle_cvi_)); #endif //CONFIG_USE_IFCI_CONFIGURATION #ifdef CONFIG_USE_PULSING_CONFIGURATION AddNewClientEntry (param_find("VTQ_PULSE_V_MODE"), - &(_pulsing_rectangular_input_parser_client->pulsing_voltage_mode_)); - AddNewClientEntry(param_find("VTQ_X_CVI"), &(_ifci_client->x_cvi_)); - AddNewClientEntry(param_find("VTQ_Y_CVI"), &(_ifci_client->y_cvi_)); - AddNewClientEntry(param_find("VTQ_ZERO_ANGLE"), &(_voltage_superposition_client->zero_angle_)); + &(_pulsing_rectangular_input_parser_client.pulsing_voltage_mode_)); + AddNewClientEntry(param_find("VTQ_X_CVI"), &(_ifci_client.x_cvi_)); + AddNewClientEntry(param_find("VTQ_Y_CVI"), &(_ifci_client.y_cvi_)); + AddNewClientEntry(param_find("VTQ_ZERO_ANGLE"), &(_voltage_superposition_client.zero_angle_)); AddNewClientEntry(param_find("VTQ_VELO_CUTOFF"), - &(_voltage_superposition_client->velocity_cutoff_)); + &(_voltage_superposition_client.velocity_cutoff_)); AddNewClientEntry(param_find("VTQ_TQUE_OFF_ANG"), - &(_voltage_superposition_client->propeller_torque_offset_angle_)); + &(_voltage_superposition_client.propeller_torque_offset_angle_)); AddNewClientEntry(param_find("VTQ_PULSE_V_LIM"), - &(_pulsing_rectangular_input_parser_client->pulsing_voltage_limit_)); + &(_pulsing_rectangular_input_parser_client.pulsing_voltage_limit_)); #endif //CONFIG_USE_PULSING_CONFIGURATION } @@ -91,15 +102,15 @@ void VertiqConfigurationHandler::UpdateClientsToNewObjId(uint8_t new_object_id) { _object_id_now = new_object_id; - DestroyAndRecreateClient(_prop_input_parser_client, new_object_id); + _prop_input_parser_client.UpdateEntryIds(new_object_id); #ifdef CONFIG_USE_IFCI_CONFIGURATION - DestroyAndRecreateClient(_ifci_client, new_object_id); + _ifci_client.UpdateEntryIds(new_object_id); #endif #ifdef CONFIG_USE_PULSING_CONFIGURATION - DestroyAndRecreateClient(_voltage_superposition_client, new_object_id); - DestroyAndRecreateClient(_pulsing_rectangular_input_parser_client, new_object_id); + _voltage_superposition_client.UpdateEntryIds(new_object_id); + _pulsing_rectangular_input_parser_client.UpdateEntryIds(new_object_id); #endif } @@ -114,8 +125,6 @@ void VertiqConfigurationHandler::UpdateIquartConfigParams() { for (uint8_t i = 0; i < _added_configuration_entry_wrappers; i++) { _configuration_entry_wrappers[i]->SendGet(_serial_interface); - //Ensure that these get messages get out - _client_manager->HandleClientCommunication(); } //Now go ahead and grab responses, and update everyone to be on the same page, but do it quickly. diff --git a/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp index 57d7afa474..5acd881dc2 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp +++ b/src/drivers/actuators/vertiq_io/vertiq_configuration_handler.hpp @@ -89,7 +89,7 @@ public: * * @param timeout The maximum amount of time we can keep trying to get responses */ - void CoordinateIquartWithPx4Params(hrt_abstime timeout = 100_ms); + void CoordinateIquartWithPx4Params(hrt_abstime timeout = 10_ms); /** * @brief Gives access to the object ID currently being used @@ -157,16 +157,16 @@ private: //////////////////////////////////////////////////////////////////////// //Vertiq Client information //Known Configuration Clients can be created as pointers to certain types of clients - EscPropellerInputParserClient *_prop_input_parser_client; + EscPropellerInputParserClient _prop_input_parser_client; #ifdef CONFIG_USE_IFCI_CONFIGURATION //Make all of the clients that we need to talk to the IFCI config params - IQUartFlightControllerInterfaceClient *_ifci_client; + IQUartFlightControllerInterfaceClient _ifci_client; #endif //CONFIG_USE_IFCI_CONFIGURATION #ifdef CONFIG_USE_PULSING_CONFIGURATION - VoltageSuperPositionClient *_voltage_superposition_client; - PulsingRectangularInputParserClient *_pulsing_rectangular_input_parser_client; + VoltageSuperPositionClient _voltage_superposition_client; + PulsingRectangularInputParserClient _pulsing_rectangular_input_parser_client; #endif //CONFIG_USE_PULSING_CONFIGURATION //////////////////////////////////////////////////////////////////////// diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.cpp b/src/drivers/actuators/vertiq_io/vertiq_io.cpp index 23269d4080..10a9dd1a18 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_io.cpp +++ b/src/drivers/actuators/vertiq_io/vertiq_io.cpp @@ -153,7 +153,7 @@ void VertiqIo::Run() void VertiqIo::parameters_update() { - //If someone has changed any parameter in our module. Checked at 1Hz + //If someone has changed any parameter in our module. Checked periodically if (_parameter_update_sub.updated()) { //Grab the changed parameter with copy (which lowers the "changed" flag) parameter_update_s param_update; diff --git a/src/drivers/actuators/vertiq_io/vertiq_io.hpp b/src/drivers/actuators/vertiq_io/vertiq_io.hpp index 379315d0e7..adbf5f9e16 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_io.hpp +++ b/src/drivers/actuators/vertiq_io/vertiq_io.hpp @@ -167,7 +167,7 @@ private: uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; //We want to publish our ESC Status to anyone who will listen // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 10_ms}; //We need to know what's going on with the actuator test to make sure we handle it properly uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; diff --git a/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp b/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp index c62327f00e..3abfed596b 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp +++ b/src/drivers/actuators/vertiq_io/vertiq_serial_interface.cpp @@ -203,7 +203,7 @@ void VertiqSerialInterface::ProcessSerialRx(ClientAbstract **client_array, uint8 ReOpenSerial(); //We have bytes - if (CheckForRx()) { + while (CheckForRx()) { uint8_t *data_ptr = ReadAndSetRxBytes(); //While we've got packets to look at, give the packet to each of the clients so that each diff --git a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp index fb2b5bd2e1..ff5e7d267d 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp +++ b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp @@ -35,7 +35,8 @@ VertiqTelemetryManager::VertiqTelemetryManager(VertiqClientManager *client_manager) : _client_manager(client_manager), - _telem_state(UNPAUSED) + _telem_state(UNPAUSED), + _telem_interface(0) { } @@ -45,8 +46,8 @@ void VertiqTelemetryManager::Init(uint64_t telem_bitmask, uint8_t module_id) _telem_bitmask = telem_bitmask; FindTelemetryModuleIds(); - _telem_interface = new IQUartFlightControllerInterfaceClient(module_id); - _client_manager->AddNewClient(_telem_interface); + _telem_interface.UpdateEntryIds(module_id); + _client_manager->AddNewClient(&_telem_interface); } void VertiqTelemetryManager::FindTelemetryModuleIds() @@ -106,9 +107,9 @@ uint16_t VertiqTelemetryManager::UpdateTelemetry() bool timed_out = (time_now - _time_of_last_telem_request) > _telem_timeout; //We got a telemetry response - if (_telem_interface->telemetry_.IsFresh()) { + if (_telem_interface.telemetry_.IsFresh()) { //grab the data - IFCITelemetryData telem_response = _telem_interface->telemetry_.get_reply(); + IFCITelemetryData telem_response = _telem_interface.telemetry_.get_reply(); // also update our internal report for logging _esc_status.esc[_current_module_id_target_index].esc_address = _module_ids_in_use[_number_of_module_ids_for_telem]; @@ -148,9 +149,8 @@ uint16_t VertiqTelemetryManager::UpdateTelemetry() uint16_t next_telem = FindNextMotorForTelemetry(); if (next_telem != _impossible_module_id) { - //We need to update the module ID we're going to listen to. So, kill the old one, and make it anew. - delete _telem_interface; - _telem_interface = new IQUartFlightControllerInterfaceClient(next_telem); + //We need to update the module ID we're going to listen to + _telem_interface.UpdateEntryIds(next_telem); } //update the telem target diff --git a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp index 8d56d33a43..21d4854e0d 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp +++ b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.hpp @@ -133,7 +133,7 @@ private: VertiqClientManager *_client_manager; vertiq_telemetry_pause_states _telem_state; //Keep track of whether or not we've paused telemetry - IQUartFlightControllerInterfaceClient *_telem_interface; //Used for reading responses from our telemetry targets + IQUartFlightControllerInterfaceClient _telem_interface; //Used for reading responses from our telemetry targets esc_status_s _esc_status; //We want to publish our ESC Status to anyone who will listen static const uint8_t MAX_SUPPORTABLE_MODULE_IDS = 63; //[0, 62] //The max number of module IDs that we can support static const uint8_t MAX_ESC_STATUS_ENTRIES =