From 7ebea55146c2a0e957798d2cb221ed47fa35eca8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Thu, 8 Jan 2026 18:56:49 +0000 Subject: [PATCH 1/5] Simulators: Add Power Task simulation for controlling power channels and monitoring states. --- src/Simulators/Power/Task.cmake | 0 src/Simulators/Power/Task.cpp | 294 ++++++++++++++++++++++++++++++++ 2 files changed, 294 insertions(+) create mode 100644 src/Simulators/Power/Task.cmake create mode 100644 src/Simulators/Power/Task.cpp diff --git a/src/Simulators/Power/Task.cmake b/src/Simulators/Power/Task.cmake new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Simulators/Power/Task.cpp b/src/Simulators/Power/Task.cpp new file mode 100644 index 0000000000..a3abc3bd60 --- /dev/null +++ b/src/Simulators/Power/Task.cpp @@ -0,0 +1,294 @@ +//*************************************************************************** +// Copyright 2007-2025 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Pedro Gonçalves * +//*************************************************************************** + +// ISO C++ 98 headers. +#include +#include +#include +#include + +// DUNE headers. +#include + +namespace Simulators +{ + //! Task to Simulate a Power Board to control power channels and monitor voltage and current. + namespace Power + { + using DUNE_NAMESPACES; + + // Maximum number of power channels. + static const int c_max_power_channel = 12; + + //! %Task arguments. + struct Arguments + { + // Channel names to simulate. + std::vector power_channel_names; + // Medium Voltage for each channel. + std::vector medium_voltage; + // Medium Current for each channel. + std::vector medium_current; + // Period to update power channels states. + double update_period; + }; + + struct Task: public Tasks::Task + { + //! Task arguments. + Arguments m_args; + //! Delay timer. + Counter m_delay_timer; + //! Flag to control no power channel to simulate. + bool m_no_power_channel; + //! IMC Power Channel messages. + IMC::PowerChannelState m_power_channels[c_max_power_channel]; + //! IMC Voltage messages. + IMC::Voltage m_voltage_msgs[c_max_power_channel]; + //! IMC Current messages. + IMC::Current m_current_msgs[c_max_power_channel]; + //! Fuel Level message. + IMC::FuelLevel m_fuel_level; + + //! Constructor. + //! @param[in] name task name. + //! @param[in] ctx context. + Task(const std::string& name, Tasks::Context& ctx): + DUNE::Tasks::Task(name, ctx) + { + param("Power Channel Names", m_args.power_channel_names) + .defaultValue("") + .description("Names of the power channels to simulate."); + + param("Medium Voltage", m_args.medium_voltage) + .defaultValue("12.0") + .description("Medium voltage (V) for each power channel."); + + param("Medium Current", m_args.medium_current) + .defaultValue("1.0") + .description("Medium current (A) for each power channel."); + + param("Update Period", m_args.update_period) + .defaultValue("1") + .minimumValue("0.1") + .description("Period, in seconds, to update power channels states."); + + bind(this); + bind(this); + } + + //! Update internal state with new parameter values. + void + onUpdateParameters(void) + { } + + unsigned int + getEid(const std::string& label) + { + unsigned int eid = 0; + try + { + eid = resolveEntity(label); + } + catch (Entities::EntityDataBase::NonexistentLabel& e) + { + (void)e; + eid = reserveEntity(label); + } + return eid; + } + + //! Reserve entity identifiers. + void + onEntityReservation(void) + { + if (m_args.power_channel_names.empty()) + { + m_no_power_channel = true; + inf("No power channel to simulate."); + return; + } + else + { + inf("Reserving power channel entities %ld.", m_args.power_channel_names.size()); + m_no_power_channel = false; + for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) + { + unsigned int eid = getEid(m_args.power_channel_names[i]); + m_power_channels[i].setSourceEntity(eid); + m_power_channels[i].name = m_args.power_channel_names[i]; + m_power_channels[i].state = IMC::PowerChannelState::PCS_ON; + inf("Power channel entity '%s' reserved with id %u.", m_power_channels[i].name.c_str(), + eid); + // Voltage message + m_voltage_msgs[i].setSourceEntity(eid); + m_voltage_msgs[i].value = m_args.medium_voltage[i]; + // Current message + m_current_msgs[i].setSourceEntity(eid); + m_current_msgs[i].value = m_args.medium_current[i]; + + m_fuel_level.setSourceEntity(getEid("Batteries")); + } + } + } + + //! Resolve entity names. + void + onEntityResolution(void) + { } + + //! Acquire resources. + void + onResourceAcquisition(void) + { } + + //! Initialize resources. + void + onResourceInitialization(void) + { + setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT); + m_delay_timer.setTop(m_args.update_period); + } + + //! Release resources. + void + onResourceRelease(void) + { } + + void + consume(const IMC::QueryPowerChannelState* msg) + { + (void)msg; + for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) + { + dispatch(m_power_channels[i]); + } + } + + void + consume(const IMC::PowerChannelControl* msg) + { + for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) + { + if (msg->name == m_power_channels[i].name) + { + switch (msg->op) + { + case IMC::PowerChannelControl::PCC_OP_TURN_OFF: + m_power_channels[i].state = IMC::PowerChannelState::PCS_OFF; + inf("Power channel '%s' turned OFF.", m_power_channels[i].name.c_str()); + break; + case IMC::PowerChannelControl::PCC_OP_TURN_ON: + m_power_channels[i].state = IMC::PowerChannelState::PCS_ON; + inf("Power channel '%s' turned ON.", m_power_channels[i].name.c_str()); + break; + case IMC::PowerChannelControl::PCC_OP_TOGGLE: + if (m_power_channels[i].state == IMC::PowerChannelState::PCS_ON) + { + m_power_channels[i].state = IMC::PowerChannelState::PCS_OFF; + inf("Power channel '%s' toggled to OFF.", m_power_channels[i].name.c_str()); + } + else + { + m_power_channels[i].state = IMC::PowerChannelState::PCS_ON; + inf("Power channel '%s' toggled to ON.", m_power_channels[i].name.c_str()); + } + break; + default: + war("Power channel control operation %u not implemented.", msg->op); + break; + } + dispatch(m_power_channels[i]); + return; + } + } + war("Power channel control received for unknown channel '%s'.", msg->name.c_str()); + } + + // Add or remove a value between 0.05 and 0.3 + double + randomFluctuation(double base_value) + { + double fluctuation = ((rand() % 26) + 5) / 100.0; // Random value between 0.05 and 0.3 + if (rand() % 2 == 0) + return base_value + fluctuation; + else + return base_value - fluctuation; + } + + //! Main loop. + void + onMain(void) + { + while (!stopping()) + { + if (m_delay_timer.overflow()) + { + m_delay_timer.reset(); + if (!m_no_power_channel) + { + for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) + { + // Dispatch voltage message + //check if channel is ON + if (m_power_channels[i].state == IMC::PowerChannelState::PCS_OFF) + { + m_voltage_msgs[i].value = 0.0; + m_current_msgs[i].value = 0.0; + dispatch(m_voltage_msgs[i]); + dispatch(m_current_msgs[i]); + } + else + { + m_voltage_msgs[i].value = randomFluctuation(m_args.medium_voltage[i]); + dispatch(m_voltage_msgs[i]); + // Dispatch current message + m_current_msgs[i].value = randomFluctuation(m_args.medium_current[i]); + dispatch(m_current_msgs[i]); + } + m_fuel_level.value = 25.0 + (rand() % 74); // Random fuel level between 75% and 100% + m_fuel_level.confidence = 0.9f; + m_fuel_level.opmodes = "SIMULATED"; + dispatch(m_fuel_level); + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); + } + } + else + { + setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INTERNAL_ERROR); + } + } + waitForMessages(0.2); + } + } + }; + } +} + +DUNE_TASK From 106b15ff79087a19353cc41c96602f1969fa701f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Thu, 8 Jan 2026 19:00:24 +0000 Subject: [PATCH 2/5] config/asv: Simulators: Add Power Simulator configuration with channel names and parameters. --- etc/asv/simulator.ini | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/etc/asv/simulator.ini b/etc/asv/simulator.ini index 93c3fd3808..027e4b5113 100644 --- a/etc/asv/simulator.ini +++ b/etc/asv/simulator.ini @@ -130,6 +130,16 @@ Entity Label = Starboard Motor Thruster Id = 1 Thruster Act to RPM Factor = 1.0, 2000.0 +[Simulators.Power] +Enabled = Never +Entity Label = Power Simulator +Power Channel Names = SADC, + Seatrac, + Auxiliary CPU +Medium Voltage = 12.0, 5.0, 24.0 +Medium Current = 1.0, 0.7, 1.6 +Update Period = 1.0 + [Simulators.Servos] Enabled = Never Entity Label = Servo From fc7051bd1a2d7d419f38226dd70802461e571786 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Thu, 8 Jan 2026 19:00:38 +0000 Subject: [PATCH 3/5] config/auv: Simulators: Add Power Simulator configuration with channel names and parameters. --- etc/auv/simulator.ini | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/etc/auv/simulator.ini b/etc/auv/simulator.ini index 5aaaea9d67..b7135d86c7 100644 --- a/etc/auv/simulator.ini +++ b/etc/auv/simulator.ini @@ -161,3 +161,13 @@ Default Speed Down = 0.0 Stream Velocity Source = Constant # Stream Velocity Source = Gridded 2D Model Data Execution Frequency = 1 + +[Simulators.Power] +Enabled = Simulation +Entity Label = Power Simulator +Power Channel Names = SADC, + Seatrac, + Auxiliary CPU +Medium Voltage = 12.0, 5.0, 24.0 +Medium Current = 1.0, 0.7, 1.6 +Update Period = 1.0 \ No newline at end of file From 7d674eea413e890128dd84de838c14f1d865efda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Tue, 13 Jan 2026 11:05:50 +0000 Subject: [PATCH 4/5] Transports/HTTP: add missing op modes for power channel control. --- src/Transports/HTTP/Task.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Transports/HTTP/Task.cpp b/src/Transports/HTTP/Task.cpp index acde95b153..bc8e857771 100644 --- a/src/Transports/HTTP/Task.cpp +++ b/src/Transports/HTTP/Task.cpp @@ -460,6 +460,14 @@ namespace Transports pcc.op = IMC::PowerChannelControl::PCC_OP_SCHED_OFF; pcc.sched_time = sched_time; } + else if (parts[0] == "reset") + { + pcc.op = IMC::PowerChannelControl::PCC_OP_SCHED_RESET; + } + else if (parts[0] == "toggle") + { + pcc.op = IMC::PowerChannelControl::PCC_OP_TOGGLE; + } sendResponse200(sock); dispatch(pcc); From c86f60bdcc3987517b85bbbdf4bb3e4b38a36567 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Tue, 13 Jan 2026 11:08:03 +0000 Subject: [PATCH 5/5] Simulators/Power: add scheduling capabilities. --- src/Simulators/Power/Task.cpp | 105 ++++++++++++++++++++++++++++------ 1 file changed, 87 insertions(+), 18 deletions(-) diff --git a/src/Simulators/Power/Task.cpp b/src/Simulators/Power/Task.cpp index a3abc3bd60..0594ac9830 100644 --- a/src/Simulators/Power/Task.cpp +++ b/src/Simulators/Power/Task.cpp @@ -46,6 +46,18 @@ namespace Simulators // Maximum number of power channels. static const int c_max_power_channel = 12; + //! Power Channel data structure. + struct PowerChannel + { + IMC::PowerChannelState pcs_msg; + unsigned id = 0; + double sched_time_in_seconds; + bool is_sched_off; + bool is_sched_on; + bool is_scheduled_active; + Counter sched_timer; + }; + //! %Task arguments. struct Arguments { @@ -68,7 +80,7 @@ namespace Simulators //! Flag to control no power channel to simulate. bool m_no_power_channel; //! IMC Power Channel messages. - IMC::PowerChannelState m_power_channels[c_max_power_channel]; + PowerChannel m_power_channels[c_max_power_channel]; //! IMC Voltage messages. IMC::Voltage m_voltage_msgs[c_max_power_channel]; //! IMC Current messages. @@ -141,10 +153,10 @@ namespace Simulators for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) { unsigned int eid = getEid(m_args.power_channel_names[i]); - m_power_channels[i].setSourceEntity(eid); - m_power_channels[i].name = m_args.power_channel_names[i]; - m_power_channels[i].state = IMC::PowerChannelState::PCS_ON; - inf("Power channel entity '%s' reserved with id %u.", m_power_channels[i].name.c_str(), + m_power_channels[i].pcs_msg.setSourceEntity(eid); + m_power_channels[i].pcs_msg.name = m_args.power_channel_names[i]; + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_ON; + inf("Power channel entity '%s' reserved with id %u.", m_power_channels[i].pcs_msg.name.c_str(), eid); // Voltage message m_voltage_msgs[i].setSourceEntity(eid); @@ -187,7 +199,7 @@ namespace Simulators (void)msg; for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) { - dispatch(m_power_channels[i]); + dispatch(m_power_channels[i].pcs_msg); } } @@ -196,35 +208,67 @@ namespace Simulators { for (int i = 0; i < (int)m_args.power_channel_names.size(); ++i) { - if (msg->name == m_power_channels[i].name) + if (msg->name == m_power_channels[i].pcs_msg.name) { switch (msg->op) { case IMC::PowerChannelControl::PCC_OP_TURN_OFF: - m_power_channels[i].state = IMC::PowerChannelState::PCS_OFF; - inf("Power channel '%s' turned OFF.", m_power_channels[i].name.c_str()); + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_OFF; + inf("Power channel '%s' turned OFF.", m_power_channels[i].pcs_msg.name.c_str()); break; + case IMC::PowerChannelControl::PCC_OP_TURN_ON: - m_power_channels[i].state = IMC::PowerChannelState::PCS_ON; - inf("Power channel '%s' turned ON.", m_power_channels[i].name.c_str()); + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_ON; + inf("Power channel '%s' turned ON.", m_power_channels[i].pcs_msg.name.c_str()); break; + case IMC::PowerChannelControl::PCC_OP_TOGGLE: - if (m_power_channels[i].state == IMC::PowerChannelState::PCS_ON) + if (m_power_channels[i].pcs_msg.state == IMC::PowerChannelState::PCS_ON) { - m_power_channels[i].state = IMC::PowerChannelState::PCS_OFF; - inf("Power channel '%s' toggled to OFF.", m_power_channels[i].name.c_str()); + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_OFF; + inf("Power channel '%s' toggled to OFF.", m_power_channels[i].pcs_msg.name.c_str()); } else { - m_power_channels[i].state = IMC::PowerChannelState::PCS_ON; - inf("Power channel '%s' toggled to ON.", m_power_channels[i].name.c_str()); + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_ON; + inf("Power channel '%s' toggled to ON.", m_power_channels[i].pcs_msg.name.c_str()); } break; + + case IMC::PowerChannelControl::PCC_OP_SCHED_OFF: + m_power_channels[i].is_scheduled_active = true; + m_power_channels[i].is_sched_off = true; + m_power_channels[i].is_sched_on = false; + m_power_channels[i].sched_time_in_seconds = msg->sched_time; + m_power_channels[i].sched_timer.setTop(m_power_channels[i].sched_time_in_seconds); + m_power_channels[i].sched_timer.reset(); + war("Power channel '%s' scheduled turn OFF in %.0f seconds.", + m_power_channels[i].pcs_msg.name.c_str(), m_power_channels[i].sched_time_in_seconds); + break; + + case IMC::PowerChannelControl::PCC_OP_SCHED_ON: + m_power_channels[i].is_scheduled_active = true; + m_power_channels[i].is_sched_on = true; + m_power_channels[i].is_sched_off = false; + m_power_channels[i].sched_time_in_seconds = msg->sched_time; + m_power_channels[i].sched_timer.setTop(m_power_channels[i].sched_time_in_seconds); + m_power_channels[i].sched_timer.reset(); + war("Power channel '%s' scheduled turn ON in %.0f seconds.", + m_power_channels[i].pcs_msg.name.c_str(), m_power_channels[i].sched_time_in_seconds); + break; + + case IMC::PowerChannelControl::PCC_OP_SCHED_RESET: + m_power_channels[i].is_scheduled_active = false; + m_power_channels[i].is_sched_off = false; + m_power_channels[i].is_sched_on = false; + war("Power channel '%s' scheduled operations RESET.", m_power_channels[i].pcs_msg.name.c_str()); + break; + default: war("Power channel control operation %u not implemented.", msg->op); break; } - dispatch(m_power_channels[i]); + dispatch(m_power_channels[i].pcs_msg); return; } } @@ -257,7 +301,7 @@ namespace Simulators { // Dispatch voltage message //check if channel is ON - if (m_power_channels[i].state == IMC::PowerChannelState::PCS_OFF) + if (m_power_channels[i].pcs_msg.state == IMC::PowerChannelState::PCS_OFF) { m_voltage_msgs[i].value = 0.0; m_current_msgs[i].value = 0.0; @@ -276,6 +320,31 @@ namespace Simulators m_fuel_level.confidence = 0.9f; m_fuel_level.opmodes = "SIMULATED"; dispatch(m_fuel_level); + + // check if scheduled operations are active + if(m_power_channels[i].is_scheduled_active) + { + if(m_power_channels[i].sched_timer.overflow()) + { + if(m_power_channels[i].is_sched_off) + { + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_OFF; + war("Power channel '%s' scheduled turn OFF executed.", m_power_channels[i].pcs_msg.name.c_str()); + } + else if(m_power_channels[i].is_sched_on) + { + m_power_channels[i].pcs_msg.state = IMC::PowerChannelState::PCS_ON; + war("Power channel '%s' scheduled turn ON executed.", m_power_channels[i].pcs_msg.name.c_str()); + } + // reset scheduling flags + m_power_channels[i].is_scheduled_active = false; + m_power_channels[i].is_sched_off = false; + m_power_channels[i].is_sched_on = false; + // dispatch updated state + dispatch(m_power_channels[i].pcs_msg); + } + } + setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE); } }