mirror of
https://github.com/eclipse-openvehicle-api/openvehicle-api.git
synced 2026-02-05 15:18:45 +00:00
322 lines
9.3 KiB
C++
322 lines
9.3 KiB
C++
/**
|
|
* @file model.cpp
|
|
* @date 2025-04-16 09:03:47
|
|
* This file defines the data link object between CAN and the V-API devices.
|
|
* This file was generated by the DBC utility from:
|
|
* datalink_demo_example.dbc
|
|
* DBC file version: 1.0.0.1
|
|
*/
|
|
|
|
#include <memory>
|
|
#include <vector>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <support/timer.h>
|
|
#include "signal_identifier.h"
|
|
#include <support/signal_support.h>
|
|
#include <support/app_control.h>
|
|
|
|
// we need to activate the counter steering service through the complex service
|
|
#include "countersteering.h"
|
|
|
|
sdv::core::CSignal g_signalSpeed;
|
|
sdv::core::CSignal g_signalSteeringWheel;
|
|
sdv::core::CSignal g_signalRearAngle;
|
|
sdv::core::CSignal g_signalIsActiveCounter;
|
|
|
|
// in case the simulation timer should be used
|
|
sdv::core::ITimerSimulationStep* g_pTimerSimulationStep;
|
|
|
|
std::unique_ptr<sdv::app::CAppControl> g_appcontrol;
|
|
|
|
bool InitializeAppControl(const std::string& resource, const std::string& configFileName)
|
|
{
|
|
auto bResult = g_appcontrol->AddModuleSearchDir( resource );
|
|
bResult &= g_appcontrol->Startup("");
|
|
g_appcontrol->SetConfigMode();
|
|
bResult &= g_appcontrol->AddConfigSearchDir( resource );
|
|
|
|
if (!configFileName.empty())
|
|
{
|
|
bResult &= g_appcontrol->LoadConfig(configFileName.c_str()) == sdv::core::EConfigProcessResult::successful;
|
|
}
|
|
|
|
return bResult;
|
|
}
|
|
|
|
sdv::core::EConfigProcessResult RegisterAllSignals()
|
|
{
|
|
std::string msg = "register all signals: ";
|
|
sdv::core::CDispatchService dispatch;
|
|
|
|
g_signalSpeed = dispatch.RegisterRxSignal("CAN_Input.Speed");
|
|
g_signalSteeringWheel = dispatch.RegisterRxSignal("CAN_Input.SteeringWheel");
|
|
g_signalRearAngle = dispatch.RegisterTxSignal("CAN_Output.RearAngle",0);
|
|
g_signalIsActiveCounter = dispatch.RegisterTxSignal("CAN_Output.IsActiveCounter",0);
|
|
|
|
if (g_signalSpeed && g_signalSteeringWheel && g_signalRearAngle && g_signalIsActiveCounter)
|
|
{
|
|
return sdv::core::EConfigProcessResult::successful;
|
|
}
|
|
|
|
return sdv::core::EConfigProcessResult::failed;
|
|
}
|
|
|
|
bool ResetAllSignals()
|
|
{
|
|
sdv::core::CDispatchService dispatch;
|
|
|
|
if (g_signalSpeed)
|
|
{
|
|
g_signalSpeed.Reset();
|
|
}
|
|
if (g_signalSteeringWheel)
|
|
{
|
|
g_signalSteeringWheel.Reset();
|
|
}
|
|
if (g_signalRearAngle)
|
|
{
|
|
g_signalRearAngle.Reset();
|
|
}
|
|
if (g_signalIsActiveCounter)
|
|
{
|
|
g_signalIsActiveCounter.Reset();
|
|
}
|
|
|
|
SDV_LOG_INFO("Reset signals");
|
|
return true;
|
|
}
|
|
|
|
|
|
bool CreateCoreServiceTomlFile(const std::string& resources)
|
|
{
|
|
std::ofstream tomlFile("sdv_core_reloc.toml");
|
|
if (tomlFile.is_open())
|
|
{
|
|
tomlFile << "# Location of the SDV binaries and configuration files\ndirectory = \"";
|
|
tomlFile << resources;
|
|
tomlFile << "\"\n";
|
|
tomlFile.close();
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
bool OpenAPILoad(const std::string& resources)
|
|
{
|
|
bool success = CreateCoreServiceTomlFile(resources);
|
|
g_appcontrol = std::make_unique<sdv::app::CAppControl> ();
|
|
|
|
//
|
|
// TODO: Dispatch service must be loaded first, adjust the correct toml file
|
|
//
|
|
success &= InitializeAppControl(resources, "data_dispatch_config_file.toml");
|
|
if (!success)
|
|
{
|
|
SDV_LOG_ERROR("Failed InitializeAppControl");
|
|
return false;
|
|
}
|
|
success &= RegisterAllSignals() == sdv::core::EConfigProcessResult::successful;
|
|
if (!success)
|
|
{
|
|
SDV_LOG_ERROR("Signals could not be registered");
|
|
}
|
|
|
|
//
|
|
//
|
|
// TODO: Load all configurations files
|
|
//
|
|
//
|
|
//
|
|
// Get the simulation task timer service if the simulation timer should be used
|
|
success &= g_appcontrol->LoadConfig("simulation_task_timer_config_file.toml") == sdv::core::EConfigProcessResult::successful;
|
|
g_pTimerSimulationStep = sdv::core::GetObject<sdv::core::ITimerSimulationStep>("SimulationTaskTimerService");
|
|
if (!g_pTimerSimulationStep)
|
|
{
|
|
SDV_LOG_WARNING("Simulation timer step not available, use normal task timer ");
|
|
success &= g_appcontrol->LoadConfig("fmu_task_timer_example.toml") == sdv::core::EConfigProcessResult::successful;
|
|
}
|
|
|
|
success &= g_appcontrol->LoadConfig("fmu_vehicle_devices_basic_services.toml") == sdv::core::EConfigProcessResult::successful;
|
|
success &= g_appcontrol->LoadConfig("fmu_complex_service.toml") == sdv::core::EConfigProcessResult::successful;
|
|
|
|
g_appcontrol->SetRunningMode();
|
|
|
|
// we need to activate the counter steering service through the complex service
|
|
auto pCounterSteeringSvc = sdv::core::GetObject("Counter Steering Example Service").GetInterface<ICounterSteeringService>();
|
|
if (pCounterSteeringSvc)
|
|
{
|
|
pCounterSteeringSvc->ActivateService(true);
|
|
}
|
|
|
|
return success;
|
|
}
|
|
|
|
void OpenAPIShutdown()
|
|
{
|
|
ResetAllSignals();
|
|
}
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
#include <float.h> // for DBL_EPSILON
|
|
#include <math.h> // for fabs()
|
|
#include "config.h"
|
|
#include "model.h"
|
|
|
|
Status cleanup(ModelInstance*)
|
|
{
|
|
SDV_LOG_INFO("Shutting down...");
|
|
OpenAPIShutdown();
|
|
return OK;
|
|
}
|
|
|
|
|
|
bool setStartValues(ModelInstance* comp)
|
|
{
|
|
std::string path(comp->resourceLocation);
|
|
std::string resourcePath = path.substr(8);
|
|
std::replace(resourcePath.begin(), resourcePath.end(), '\\', '/');
|
|
if (!OpenAPILoad(resourcePath))
|
|
{
|
|
logError(comp, "Could not initialze Core Instance: %s", resourcePath.c_str());
|
|
comp->terminateSimulation = true;
|
|
return false;
|
|
}
|
|
|
|
// TODO: move this to initialize()?
|
|
comp->nextEventTime = 0;
|
|
comp->nextEventTimeDefined = true;
|
|
return true;
|
|
}
|
|
|
|
//void reset(ModelInstance* comp)
|
|
//{
|
|
// OpenAPIShutdown();
|
|
// OpenAPILoad((std::string(comp->resourceLocation) + "repository_service.sdv").c_str());
|
|
//}
|
|
|
|
Status calculateValues(ModelInstance* comp)
|
|
{
|
|
UNUSED(comp);
|
|
// nothing to do
|
|
return OK;
|
|
}
|
|
|
|
Status getFloat64(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] double values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
|
|
{
|
|
ASSERT_NVALUES(1);
|
|
|
|
switch (vr)
|
|
{
|
|
case vr_Speed:
|
|
values[(*index)++] = M(Speed);
|
|
break;
|
|
case vr_SteeringWheel:
|
|
values[(*index)++] = M(SteeringWheel);
|
|
break;
|
|
case vr_RearAngle:
|
|
values[(*index)++] = M(RearAngle);
|
|
break;
|
|
|
|
default:
|
|
logError(comp, "Get Float64 is not allowed for value reference u.", vr);
|
|
return Error;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
Status getInt32(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] int32_t values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
|
|
{
|
|
ASSERT_NVALUES(1);
|
|
|
|
switch (vr)
|
|
{
|
|
case vr_IsActiveCounter:
|
|
values[(*index)++] = M(IsActiveCounter);
|
|
break;
|
|
|
|
default:
|
|
logError(comp, "Get Int32 is not allowed for value reference u.", vr);
|
|
return Error;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
Status setFloat64(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] const double values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
|
|
{
|
|
ASSERT_NVALUES(1);
|
|
|
|
switch (vr)
|
|
{
|
|
case vr_Speed:
|
|
M(Speed) = values[(*index)++];
|
|
break;
|
|
case vr_SteeringWheel:
|
|
M(SteeringWheel) = values[(*index)++];
|
|
break;
|
|
case vr_RearAngle:
|
|
M(RearAngle) = values[(*index)++];
|
|
break;
|
|
|
|
default:
|
|
logError(comp, "Set Float64 is not allowed for value reference u.", vr);
|
|
return Error;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
Status setInt32(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] const int32_t values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
|
|
{
|
|
ASSERT_NVALUES(1);
|
|
|
|
switch (vr)
|
|
{
|
|
case vr_IsActiveCounter:
|
|
M(IsActiveCounter) = values[(*index)++];
|
|
break;
|
|
|
|
default:
|
|
logError(comp, "Set Int32 is not allowed for value reference u.", vr);
|
|
return Error;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
void eventUpdate(ModelInstance* comp)
|
|
{
|
|
|
|
if (g_pTimerSimulationStep) // in case the simulation timer was used, maybe the step size has to be adjusted
|
|
{
|
|
g_pTimerSimulationStep->SimulationStep(1000);
|
|
}
|
|
|
|
g_signalSpeed.Write( M(Speed));
|
|
g_signalSteeringWheel.Write( M(SteeringWheel));
|
|
M(RearAngle) = g_signalRearAngle.Read().get<float>();
|
|
M(IsActiveCounter) = g_signalIsActiveCounter.Read().get<uint32_t>();
|
|
|
|
double epsilon = (1.0 + fabs(comp->time)) * DBL_EPSILON;
|
|
|
|
if (comp->nextEventTimeDefined && comp->time + epsilon >= comp->nextEventTime) {
|
|
comp->nextEventTime += FIXED_SOLVER_STEP;
|
|
}
|
|
|
|
comp->valuesOfContinuousStatesChanged = false;
|
|
comp->nominalsOfContinuousStatesChanged = false;
|
|
comp->terminateSimulation = false;
|
|
comp->nextEventTimeDefined = true;
|
|
}
|
|
|
|
#ifdef __cplusplus
|
|
} // end of extern "C"
|
|
#endif
|
|
|
|
|