Precommit (#1)

* first commit * cleanup
2026-02-05 15:18:45 +00:00 · 2025-11-04 13:28:06 +01:00
parent dba45dc636
commit 6ed4b1534e
898 changed files with 256340 additions and 0 deletions
--- a/examples/system_demo_example/example_service/complex_service.cpp
+++ b/examples/system_demo_example/example_service/complex_service.cpp
@@ -0,0 +1,176 @@
+#include <iostream>
+#include "complex_service.h"
+
+const float g_fSpeedThreshold = 30.0f / 3.6f;
+
+CCounterSteeringExampleService::CCounterSteeringExampleService()
+{
+}
+
+CCounterSteeringExampleService::~CCounterSteeringExampleService()
+{
+    // Just in case...
+    Shutdown();
+}
+
+void CCounterSteeringExampleService::Initialize(const sdv::u8string& /*ssObjectConfig*/)
+{
+    m_eStatus = sdv::EObjectStatus::initializing;
+
+    // Request the basic service for monitoring the alive counter.
+    m_pAliveCounterSvc = sdv::core::GetObject("Vehicle.Software.Application.IsActiveCounter_Service").GetInterface<vss::Vehicle::Software::Application::IsActiveCounterService::IVSS_SetCounter>();
+    if (!m_pAliveCounterSvc)
+    {
+        SDV_LOG_ERROR("Could not get interface 'IVSS_SetCounter': [CCounterSteeringExampleService]");
+        m_eStatus = sdv::EObjectStatus::initialization_failure;
+        return;
+    }
+
+    // Request the basic service for the rear axle.
+    m_pRearAxleSvc = sdv::core::GetObject("Vehicle.Chassis.RearAxle.Row.Wheel_Service").GetInterface<vss::Vehicle::Chassis::RearAxle::Row::WheelService::IVSS_SetRearAxle>();
+    if (!m_pRearAxleSvc)
+    {
+        SDV_LOG_ERROR("Could not get interface 'IVSS_SetAngle': [CCounterSteeringExampleService]");
+        m_eStatus = sdv::EObjectStatus::initialization_failure;
+        return;
+    }
+
+    // Request the basic service for the steering wheel.
+    auto pSteeringWheelSvc = sdv::core::GetObject("Vehicle.Chassis.SteeringWheel.Angle_Service").GetInterface<vss::Vehicle::Chassis::SteeringWheel::AngleService::IVSS_GetSteeringWheel>();
+    if (!pSteeringWheelSvc)
+    {
+        SDV_LOG_ERROR("Could not get interface 'IVSS_SetSteeringAngle': [CCounterSteeringExampleService]");
+        m_eStatus = sdv::EObjectStatus::initialization_failure;
+        return;
+    }
+
+    // Request the basic service for the vehicle speed.
+    auto pVehSpeedSvc = sdv::core::GetObject("Vehicle.Speed_Service").GetInterface<vss::Vehicle::SpeedService::IVSS_GetSpeed>();
+    if (!pVehSpeedSvc)
+    {
+        SDV_LOG_ERROR("Could not get interface 'IVSS_SetSpeed': [CCounterSteeringExampleService]");
+        m_eStatus = sdv::EObjectStatus::initialization_failure;
+        return;
+    }
+
+    // Register steering wheel change event handler.
+    pSteeringWheelSvc->RegisterOnSignalChangeOfWheelAngle(static_cast<vss::Vehicle::Chassis::SteeringWheel::AngleService::IVSS_SetSteeringWheel_Event*> (this));
+
+    // Register vehicle speed change event handler.
+    pVehSpeedSvc->RegisterOnSignalChangeOfVehicleSpeed(static_cast<vss::Vehicle::SpeedService::IVSS_SetSpeed_Event*> (this));
+
+    // Start the alive timer with 10 ms period.
+    m_Timer = sdv::core::CTaskTimer(10, [&]() {TimerFunction(); });
+    if (!m_Timer)
+    {
+        SDV_LOG_ERROR("CCounterSteeringExampleService: tasktimer with 10 milliseconds could not be created.");
+        m_eStatus = sdv::EObjectStatus::initialization_failure;
+        return;
+    }
+
+    SDV_LOG_INFO("CCounterSteeringExampleService: tasktimer created with 10 milliseconds");
+
+    m_eStatus = sdv::EObjectStatus::initialized;
+}
+
+sdv::EObjectStatus CCounterSteeringExampleService::GetStatus() const
+{
+    return m_eStatus;
+}
+
+void CCounterSteeringExampleService::SetOperationMode(sdv::EOperationMode /*eMode*/)
+{
+    // Not applicable
+}
+
+void CCounterSteeringExampleService::Shutdown()
+{
+    // Terminate the alive counter
+    m_Timer.Reset();
+
+    // Unregister the steering wheel event handler.
+    auto pSteeringWheelSvc = sdv::core::GetObject("Vehicle.Chassis.SteeringWheel.Angle_Service").GetInterface<vss::Vehicle::Chassis::SteeringWheel::AngleService::IVSS_GetSteeringWheel>();
+    if (pSteeringWheelSvc)
+        pSteeringWheelSvc->UnregisterOnSignalChangeOfWheelAngle(static_cast<vss::Vehicle::Chassis::SteeringWheel::AngleService::IVSS_SetSteeringWheel_Event*> (this));
+
+    // Unregister the vehicle speed event handler.
+    auto pVehSpeedSvc = sdv::core::GetObject("Vehicle.Speed_Service").GetInterface<vss::Vehicle::SpeedService::IVSS_GetSpeed>();
+    if (pVehSpeedSvc)
+        pVehSpeedSvc->UnregisterOnSignalChangeOfVehicleSpeed(static_cast<vss::Vehicle::SpeedService::IVSS_SetSpeed_Event*> (this));
+}
+
+void CCounterSteeringExampleService::SetSteeringWheel(float value)
+{
+    if (m_fSteeringWheel == value) return;
+    m_fSteeringWheel = value;
+    UpdateRearAxleAngle();
+}
+
+void CCounterSteeringExampleService::SetSpeed(float value)
+{
+    if (m_fVehSpeed == value) return;
+    m_fVehSpeed = value;
+    UpdateRearAxleAngle();
+}
+
+void CCounterSteeringExampleService::ActivateService(/*in*/ bool bActivate)
+{
+    if (m_bActivated == bActivate) return;
+    m_bActivated = bActivate;
+    UpdateRearAxleAngle();
+}
+
+bool CCounterSteeringExampleService::IsActivated() const
+{
+    return m_bActivated;
+}
+
+bool CCounterSteeringExampleService::CounterSteeringActive() const
+{
+    return m_bActivated && m_fVehSpeed < g_fSpeedThreshold;
+}
+
+double CCounterSteeringExampleService::RearAxleAngle() const
+{
+    return m_fRearAxleAngle;
+}
+
+void CCounterSteeringExampleService::UpdateRearAxleAngle()
+{
+    float fRearAxleAngle = 0.0;
+    if (CounterSteeringActive())
+    {
+        // Get steering wheel angle percentage
+        // The steering wheel can have values from -16...16 rad.
+        float fSteeringWheelPercent = m_fSteeringWheel / 16.0f;
+        if (fSteeringWheelPercent > 1.0) fSteeringWheelPercent = 1.0;
+        if (fSteeringWheelPercent < -1.0) fSteeringWheelPercent = -1.0;
+
+        // Calculate the vehicle speed percentage up to 30 km/h
+        // The vehicle speed is calculated in m/s.
+        float fVehSpeedPercent = m_fVehSpeed / g_fSpeedThreshold;
+        if (fVehSpeedPercent > 1.0) fVehSpeedPercent = 1.0;
+
+        // Calculate the counter steering percentage (counter proportional to the vehicle speed - the quicker, the less counter
+        // steering).
+        // The rear axle angle can change between -5.12...5.11 deg.
+        fRearAxleAngle = 5.11f * -fSteeringWheelPercent * (1.0f - fVehSpeedPercent);
+    }
+    if (fRearAxleAngle == m_fRearAxleAngle) return;
+
+    // Inform the rear exle about the new steering position.
+    m_fRearAxleAngle = fRearAxleAngle;
+    if (m_pRearAxleSvc) m_pRearAxleSvc->SetRearAxle(fRearAxleAngle);
+}
+
+void CCounterSteeringExampleService::TimerFunction()
+{
+    // Increase the counter (max 5 bits).
+    m_uiAliveCounter++;
+    if (m_uiAliveCounter > 31)
+        m_uiAliveCounter = 0;
+
+    // Inform the alive counter monitor service
+    if (m_pAliveCounterSvc)
+        m_pAliveCounterSvc->SetCounter(m_uiAliveCounter);
+}