mirror of
https://github.com/eclipse-openvehicle-api/openvehicle-api.git
synced 2026-04-13 08:58:17 +00:00
tompzf
@@ -0,0 +1,202 @@
|
||||
#include <iostream>
|
||||
#include "autoheadlight_cs.h"
|
||||
|
||||
|
||||
CAutoHeadlightService::CAutoHeadlightService()
|
||||
{
|
||||
}
|
||||
|
||||
CAutoHeadlightService::~CAutoHeadlightService()
|
||||
{
|
||||
Shutdown();
|
||||
}
|
||||
|
||||
void CAutoHeadlightService::Initialize(const sdv::u8string& ssObjectConfig)
|
||||
{
|
||||
m_eStatus = sdv::EObjectStatus::initializing;
|
||||
|
||||
// Request the basic service for the headlight.
|
||||
m_pHeadlightSvc = sdv::core::GetObject("Vehicle.Body.Light.Front.LowBeam_Service").GetInterface<vss::Vehicle::Body::Light::Front::LowBeamService::IVSS_SetHeadLightLowBeam>();
|
||||
if (!m_pHeadlightSvc)
|
||||
{
|
||||
SDV_LOG_ERROR("Could not get interface 'IVSS_SetHeadlightLowBeam': [CAutoHeadlightService]");
|
||||
m_eStatus = sdv::EObjectStatus::initialization_failure;
|
||||
return;
|
||||
}
|
||||
|
||||
// Request the basic service for the steering wheel.
|
||||
auto pCurrentLatitudeSvc = sdv::core::GetObject("Vehicle.Position.CurrentLatitude_Service").GetInterface<vss::Vehicle::Position::CurrentLatitudeService::IVSS_GetCurrentLatitude>();
|
||||
if (!pCurrentLatitudeSvc)
|
||||
{
|
||||
SDV_LOG_ERROR("Could not get interface 'IVSS_GetCurrentLatitude': [CAutoHeadlightService]");
|
||||
m_eStatus = sdv::EObjectStatus::initialization_failure;
|
||||
return;
|
||||
}
|
||||
|
||||
// Request the basic service for the vehicle speed.
|
||||
auto pCurrentLongitudeSvc = sdv::core::GetObject("Vehicle.Position.CurrentLongitude_Service").GetInterface<vss::Vehicle::Position::CurrentLongitudeService::IVSS_GetCurrentLongitude>();
|
||||
if (!pCurrentLongitudeSvc)
|
||||
{
|
||||
SDV_LOG_ERROR("Could not get interface 'IVSS_GetCurrentLongitude': [CAutoHeadlightService]");
|
||||
m_eStatus = sdv::EObjectStatus::initialization_failure;
|
||||
return;
|
||||
}
|
||||
|
||||
// Register Current Latitude change event handler.
|
||||
pCurrentLatitudeSvc->RegisterOnSignalChangeOfFCurrentLatitude(static_cast<vss::Vehicle::Position::CurrentLatitudeService::IVSS_SetCurrentLatitude_Event*> (this));
|
||||
|
||||
// Register Current Longitude change event handler.
|
||||
pCurrentLongitudeSvc->RegisterOnSignalChangeOfFCurrentLongitude(static_cast<vss::Vehicle::Position::CurrentLongitudeService::IVSS_SetCurrentLongitude_Event*> (this));
|
||||
|
||||
|
||||
if(LoadGPSBounds(ssObjectConfig))
|
||||
{
|
||||
SDV_LOG_INFO("AutoHeadlightService: GPS bounds loaded Successfully");
|
||||
m_eStatus = sdv::EObjectStatus::initialized;
|
||||
}
|
||||
else
|
||||
{
|
||||
SDV_LOG_ERROR("AutoHeadlightService: GPS bounds could not be loaded");
|
||||
m_eStatus = sdv::EObjectStatus::initialization_failure;
|
||||
return;
|
||||
}
|
||||
|
||||
SDV_LOG_INFO("AutoHeadlightService: Initialized Successfully");
|
||||
}
|
||||
|
||||
sdv::EObjectStatus CAutoHeadlightService::GetStatus() const
|
||||
{
|
||||
return m_eStatus;
|
||||
}
|
||||
|
||||
void CAutoHeadlightService::SetOperationMode(sdv::EOperationMode /*eMode*/)
|
||||
{
|
||||
// Not applicable
|
||||
}
|
||||
|
||||
void CAutoHeadlightService::Shutdown()
|
||||
{
|
||||
// Unregister the Current latitude event handler.
|
||||
auto pCurrentLatitudeSvc = sdv::core::GetObject("Vehicle.Position.CurrentLatitude_Service").GetInterface<vss::Vehicle::Position::CurrentLatitudeService::IVSS_GetCurrentLatitude>();
|
||||
if (pCurrentLatitudeSvc)
|
||||
pCurrentLatitudeSvc->UnregisterOnSignalChangeOfFCurrentLatitude(static_cast<vss::Vehicle::Position::CurrentLatitudeService::IVSS_SetCurrentLatitude_Event*> (this));
|
||||
|
||||
// Unregister the vehicle speed event handler.
|
||||
auto pCurrentLongitudeSvc = sdv::core::GetObject("Vehicle.Position.CurrentLongitude_Service").GetInterface<vss::Vehicle::Position::CurrentLongitudeService::IVSS_GetCurrentLongitude>();
|
||||
if (pCurrentLongitudeSvc)
|
||||
pCurrentLongitudeSvc->UnregisterOnSignalChangeOfFCurrentLongitude(static_cast<vss::Vehicle::Position::CurrentLongitudeService::IVSS_SetCurrentLongitude_Event*> (this));
|
||||
}
|
||||
|
||||
void CAutoHeadlightService::SetCurrentLatitude(float value)
|
||||
{
|
||||
if (m_fCurrentLatitude == value)
|
||||
return;
|
||||
|
||||
m_fCurrentLatitude = value;
|
||||
ProcessHeadlightBasedOnEgoPosition();
|
||||
}
|
||||
|
||||
void CAutoHeadlightService::SetCurrentLongitude(float value)
|
||||
{
|
||||
if (m_fCurrentLongitude == value)
|
||||
return;
|
||||
|
||||
m_fCurrentLongitude = value;
|
||||
ProcessHeadlightBasedOnEgoPosition();
|
||||
}
|
||||
|
||||
bool CAutoHeadlightService::IsinTunnel() const
|
||||
{
|
||||
// Check if vehicle is within the tunnel bounds
|
||||
if (m_fCurrentLatitude >= m_SGPSBoundingBox.fTunnelMinLat && m_fCurrentLatitude <= m_SGPSBoundingBox.fTunnelMaxLat &&
|
||||
m_fCurrentLongitude >= m_SGPSBoundingBox.fTunnelMinLon && m_fCurrentLongitude <= m_SGPSBoundingBox.fTunnelMaxLon)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CAutoHeadlightService::GetHeadlightStatus() const
|
||||
{
|
||||
return m_bHeadlight;
|
||||
}
|
||||
|
||||
void CAutoHeadlightService::ProcessHeadlightBasedOnEgoPosition()
|
||||
{
|
||||
auto isInTunnel = IsinTunnel();
|
||||
if (isInTunnel && !m_bHeadlight)
|
||||
{
|
||||
// switch on headlight
|
||||
m_bHeadlight = true;
|
||||
m_pHeadlightSvc->SetHeadLightLowBeam(m_bHeadlight);
|
||||
}
|
||||
if (!IsinTunnel() && m_bHeadlight)
|
||||
{
|
||||
// switch off headlight
|
||||
m_bHeadlight = false;
|
||||
m_pHeadlightSvc->SetHeadLightLowBeam(m_bHeadlight);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool CAutoHeadlightService::LoadGPSBounds(const sdv::u8string& rssObjectConfig)
|
||||
{
|
||||
try
|
||||
{
|
||||
sdv::toml::CTOMLParser config(rssObjectConfig.c_str());
|
||||
|
||||
sdv::toml::CNode fStartLatNode = config.GetDirect("tunnel_start_lat");
|
||||
float fTunnelStartLat = 0.0; ///< Tunnel Start Latitude
|
||||
if (fStartLatNode.GetType() == sdv::toml::ENodeType::node_floating_point)
|
||||
{
|
||||
fTunnelStartLat = static_cast<float>(fStartLatNode.GetValue());
|
||||
}
|
||||
|
||||
sdv::toml::CNode fStartLonNode = config.GetDirect("tunnel_start_lon");
|
||||
float fTunnelStartLon = 0.0; ///< Tunnel Start Longitude
|
||||
if (fStartLonNode.GetType() == sdv::toml::ENodeType::node_floating_point)
|
||||
{
|
||||
fTunnelStartLon = static_cast<float>(fStartLonNode.GetValue());
|
||||
}
|
||||
|
||||
sdv::toml::CNode fEndLatNode = config.GetDirect("tunnel_end_lat");
|
||||
float fTunnelEndLat = 0.0; ///< Tunnel End Latitude
|
||||
if (fEndLatNode.GetType() == sdv::toml::ENodeType::node_floating_point)
|
||||
{
|
||||
fTunnelEndLat = static_cast<float>(fEndLatNode.GetValue());
|
||||
}
|
||||
|
||||
sdv::toml::CNode fEndLonNode = config.GetDirect("tunnel_end_lon");
|
||||
float fTunnelEndLon = 0.0; ///< Tunnel End Longitude
|
||||
if (fEndLonNode.GetType() == sdv::toml::ENodeType::node_floating_point)
|
||||
{
|
||||
fTunnelEndLon = static_cast<float>(fEndLonNode.GetValue());
|
||||
}
|
||||
|
||||
// Calculate bounding box
|
||||
m_SGPSBoundingBox.fTunnelMinLat = std::min(fTunnelStartLat, fTunnelEndLat);
|
||||
m_SGPSBoundingBox.fTunnelMaxLat = std::max(fTunnelStartLat, fTunnelEndLat);
|
||||
m_SGPSBoundingBox.fTunnelMinLon = std::min(fTunnelStartLon, fTunnelEndLon);
|
||||
m_SGPSBoundingBox.fTunnelMaxLon = std::max(fTunnelStartLon, fTunnelEndLon);
|
||||
}
|
||||
catch (const sdv::toml::XTOMLParseException& e)
|
||||
{
|
||||
SDV_LOG_ERROR("Parsing error: ", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
IAutoheadlightService::SGPSBoundBox CAutoHeadlightService::GetGPSBoundBox() const
|
||||
{
|
||||
SGPSBoundBox tunnel;
|
||||
tunnel.fTunnelMinLat = m_SGPSBoundingBox.fTunnelMinLat;
|
||||
tunnel.fTunnelMaxLat = m_SGPSBoundingBox.fTunnelMaxLat;
|
||||
tunnel.fTunnelMinLon = m_SGPSBoundingBox.fTunnelMinLon;
|
||||
tunnel.fTunnelMaxLon = m_SGPSBoundingBox.fTunnelMaxLon;
|
||||
|
||||
return tunnel;
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
#ifndef COMPLEX_SERVICE_EXAMPLE_H
|
||||
#define COMPLEX_SERVICE_EXAMPLE_H
|
||||
|
||||
// C++ library
|
||||
#include <iostream>
|
||||
|
||||
|
||||
// SDV framework support
|
||||
#include <support/component_impl.h>
|
||||
#include <support/signal_support.h>
|
||||
#include <support/timer.h>
|
||||
#include <support/toml.h>
|
||||
|
||||
// VSS interfaces - located in ../generated/vss_files/include
|
||||
#include "vss_vehiclepositioncurrentlatitude_bs_rx.h"
|
||||
#include "vss_vehiclepositioncurrentlongitude_bs_rx.h"
|
||||
#include "vss_vehiclebodylightfrontlowbeam_bs_tx.h"
|
||||
|
||||
|
||||
// Complex service Headlight interface - located in ../generated/example_service
|
||||
#include "autoheadlight_cs_ifc.h"
|
||||
|
||||
/**
|
||||
* @brief Auto Headlight service
|
||||
* @details This complex service enables the headlight if the vehicle position is detected inside the tunnel and disables the headlight
|
||||
* if vehicle is detected outside the tunnel. This also checks if the time based on summer and winter season and enables it accordingly.(time aspect will be developed later)
|
||||
*
|
||||
* Input events from basic service: CurrentLatitude
|
||||
* CurrentLongitude
|
||||
* Output calls for basic service: headlight (true or false)
|
||||
*
|
||||
* Input calls for applications: autoheadlight enabled (true/false)
|
||||
* Output info for applications: headlight status (true/false)
|
||||
* Inside the tunnel (true/false)
|
||||
*/
|
||||
class CAutoHeadlightService :
|
||||
public sdv::CSdvObject,
|
||||
public sdv::IObjectControl,
|
||||
public IAutoheadlightService,
|
||||
public vss::Vehicle::Position::CurrentLatitudeService::IVSS_SetCurrentLatitude_Event,
|
||||
public vss::Vehicle::Position::CurrentLongitudeService::IVSS_SetCurrentLongitude_Event
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
CAutoHeadlightService();
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
~CAutoHeadlightService();
|
||||
|
||||
// Interface map
|
||||
BEGIN_SDV_INTERFACE_MAP()
|
||||
SDV_INTERFACE_ENTRY(sdv::IObjectControl)
|
||||
SDV_INTERFACE_ENTRY(vss::Vehicle::Position::CurrentLatitudeService::IVSS_SetCurrentLatitude_Event)
|
||||
SDV_INTERFACE_ENTRY(vss::Vehicle::Position::CurrentLongitudeService::IVSS_SetCurrentLongitude_Event)
|
||||
SDV_INTERFACE_ENTRY(IAutoheadlightService)
|
||||
END_SDV_INTERFACE_MAP()
|
||||
|
||||
// Object declarations
|
||||
DECLARE_OBJECT_CLASS_TYPE(sdv::EObjectType::ComplexService)
|
||||
DECLARE_OBJECT_CLASS_NAME("Auto Headlight Service")
|
||||
DECLARE_OBJECT_SINGLETON()
|
||||
|
||||
/**
|
||||
* @brief Initialize the object. Overload of sdv::IObjectControl::Initialize.
|
||||
* @param[in] ssObjectConfig Optional configuration string.
|
||||
*/
|
||||
void Initialize(const sdv::u8string& ssObjectConfig) override;
|
||||
|
||||
/**
|
||||
* @brief Get the current status of the object. Overload of sdv::IObjectControl::GetStatus.
|
||||
* @return Return the current status of the object.
|
||||
*/
|
||||
sdv::EObjectStatus GetStatus() const override;
|
||||
|
||||
/**
|
||||
* @brief Set the component operation mode. Overload of sdv::IObjectControl::SetOperationMode.
|
||||
* @param[in] eMode The operation mode, the component should run in.
|
||||
*/
|
||||
void SetOperationMode(sdv::EOperationMode eMode) override;
|
||||
|
||||
/**
|
||||
* @brief Shutdown called before the object is destroyed. Overload of sdv::IObjectControl::Shutdown.
|
||||
*/
|
||||
void Shutdown() override;
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief Set Current latitude event. Overload of vss::Vehicle::Position::CurrentLatitudeService::IVSS_SetBSCurrentLatitude_Event
|
||||
* @param[in] value Current latitude value in float
|
||||
*/
|
||||
void SetCurrentLatitude(float value) override;
|
||||
|
||||
/**
|
||||
* @brief Set Current Longitude event. Overload of vss::Vehicle::Position::CurrentLongitudeService::IVSS_SetBSCurrentLongitude_Event
|
||||
* @param[in] value Current latitude value in float
|
||||
*/
|
||||
void SetCurrentLongitude(float value) override;
|
||||
|
||||
/**
|
||||
* @brief Gets status of vehicle position if it is inside tunnel or not. Overload of IAutoheadlightService::IsinTunnel.
|
||||
* @return Returns true if position of vehicle is inside tunnel and false if outside.
|
||||
*/
|
||||
bool IsinTunnel() const override;
|
||||
|
||||
/**
|
||||
* @brief Get status of headlights. Overload of IAutoheadlightService::GetHeadlightStatus.
|
||||
* @return Returns the status of headlights (true if switched on , false if not switched on)
|
||||
*/
|
||||
bool GetHeadlightStatus() const override;
|
||||
|
||||
/**
|
||||
* @brief Get the GPS bounding box
|
||||
* @return Returns the bounding box structure
|
||||
*/
|
||||
SGPSBoundBox GetGPSBoundBox() const override;
|
||||
|
||||
/**
|
||||
* @brief Update the headlight status based on the vehicle position with respect to tunnel.
|
||||
*/
|
||||
void ProcessHeadlightBasedOnEgoPosition();
|
||||
|
||||
/**
|
||||
* @brief Load GPS bounding box from the configuration file
|
||||
*/
|
||||
bool LoadGPSBounds(const sdv::u8string& rssObjectConfig);
|
||||
|
||||
sdv::EObjectStatus m_eStatus = sdv::EObjectStatus::initialization_pending; ///< Current object status
|
||||
volatile float m_fCurrentLatitude = 0.0; ///< Current Latitude
|
||||
volatile float m_fCurrentLongitude = 0.0; ///< Current Longitude
|
||||
volatile bool m_bHeadlight = false; ///< Headlight status
|
||||
SGPSBoundBox m_SGPSBoundingBox; ///< Tunnel bounding box coordinates
|
||||
|
||||
|
||||
///< Headlight interface.
|
||||
vss::Vehicle::Body::Light::Front::LowBeamService::IVSS_SetHeadLightLowBeam* m_pHeadlightSvc = nullptr;
|
||||
};
|
||||
|
||||
DEFINE_SDV_OBJECT(CAutoHeadlightService)
|
||||
|
||||
#endif // !define COMPLEX_SERVICE_EXAMPLE_H
|
||||
@@ -0,0 +1,42 @@
|
||||
/*******************************************************************************
|
||||
* @file AutoHeadlight.idl
|
||||
* @details Auto Headlight example service interface definition.
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief AutoHeadlight example service interface. The interface provides functions for the
|
||||
* Auto Headlight example complex service.
|
||||
*/
|
||||
interface IAutoheadlightService
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Tunnel Bounding box Coordinates struct.
|
||||
*/
|
||||
struct SGPSBoundBox
|
||||
{
|
||||
float fTunnelMinLat = 0.0; ///< Minimum latitude value of bounding box
|
||||
float fTunnelMinLon = 0.0; ///< Minimum longitude value of bounding box
|
||||
float fTunnelMaxLat = 0.0; ///< Maximum latitude value of bounding box
|
||||
float fTunnelMaxLon = 0.0; ///< Maximum longitude value of bounding box
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Gets status of vehicle position if it is inside tunnel or not
|
||||
* @return Returns true if position of vehicle is inside tunnel and false if outside.
|
||||
*/
|
||||
boolean IsinTunnel() const;
|
||||
|
||||
/**
|
||||
* @brief Get status of headlights
|
||||
* @return Returns the status of headlights (true if switched on , false if not switched on)
|
||||
*/
|
||||
boolean GetHeadlightStatus() const;
|
||||
|
||||
/**
|
||||
* @brief Get the GPS bounding box
|
||||
* @return Returns the bounding box structure
|
||||
*/
|
||||
SGPSBoundBox GetGPSBoundBox() const;
|
||||
};
|
||||
Reference in New Issue
Block a user