158 lines
4.4 KiB
C
158 lines
4.4 KiB
C
/**
|
|
* @file sdl.c
|
|
*
|
|
* @copyright Nehemis SARL reserves all rights even in the event of industrial
|
|
* property rights. We reserve all rights of disposal such as
|
|
* copying and passing on to third parties.
|
|
*
|
|
* @brief This file contains the initialization and execution logic for
|
|
* the Valve Controller and CANopen stack. It defines the main
|
|
* scheduler loop for running these components within the firmware.
|
|
*
|
|
* @todo SDL_MIN_OPERATIONAL_NODES to be changed to 20, are the node is fixed.
|
|
* (electronic issue in the board)
|
|
*
|
|
*/
|
|
|
|
/******************************************************************************
|
|
* Include Header Files
|
|
******************************************************************************/
|
|
/* CANopen includes */
|
|
#include <co_canopen.h>
|
|
#include <gen_define.h>
|
|
#include <co_canopen.h>
|
|
#include "co_sdo.h"
|
|
#include "co_odaccess.h"
|
|
|
|
/* User includes */
|
|
#include "sdl.h"
|
|
#include "processBoard.h"
|
|
#include "enduranceTestBench.h"
|
|
#include "hal_system.h"
|
|
#include "nms_can.h"
|
|
|
|
#include "hal_timer.h"
|
|
#include "stdlib.h"
|
|
#include "filter.h"
|
|
#include "pressureController.h"
|
|
/******************************************************************************
|
|
* Macro constant declarations
|
|
******************************************************************************/
|
|
|
|
/******************************************************************************
|
|
* Type Declarations
|
|
******************************************************************************/
|
|
static void SdlFilterInterrupt(void);
|
|
|
|
static inline float32 RevModelConvertToPercent(float32 value_f32, float32 minVal_f32, float32 maxVal_f32)
|
|
{
|
|
float32 clamped = value_f32;
|
|
|
|
if (clamped < minVal_f32)
|
|
{
|
|
clamped = minVal_f32;
|
|
}
|
|
else if (clamped > maxVal_f32)
|
|
{
|
|
clamped = maxVal_f32;
|
|
}
|
|
|
|
return 100.0f * (clamped - minVal_f32) / (maxVal_f32 - minVal_f32);
|
|
}
|
|
/******************************************************************************
|
|
* Global Declarations
|
|
******************************************************************************/
|
|
static bool testBenchStarted_b;
|
|
static PuControllersPidInput_st pmpRoPressurePidInput_st = {0};
|
|
static PuFiltersSensorData_st receivedFilterData_st;
|
|
/******************************************************************************
|
|
* Public Function Definitions
|
|
******************************************************************************/
|
|
void SdlInit(void)
|
|
{
|
|
testBenchStarted_b = false;
|
|
|
|
HalTimerStart(MAP_HAL_TIMER_FILTER);
|
|
NmsCanInit();
|
|
PuFiltersInit();
|
|
ProcessBoardInit();
|
|
HalTimerConfigureCallback(MAP_HAL_TIMER_FILTER, SdlFilterInterrupt);
|
|
|
|
PuControllersInitInputs(&pmpRoPressurePidInput_st);
|
|
}
|
|
|
|
|
|
void SdlRun(void)
|
|
{
|
|
static uint64 lastPressureUpdateMs_u64 = 0uLL;
|
|
uint64 currentTimeMs_u64;
|
|
static uint8 pressureSetpoint_u8 = 0u;
|
|
uint8 turnOnTestbench = 0u;
|
|
|
|
NmsCanGetObj_u8(0x2007, 0x0, &turnOnTestbench);
|
|
NmsCanRun();
|
|
|
|
// if (turnOnTestbench == 1)
|
|
// {
|
|
// if (NmsCanAreAllNodesOperational())
|
|
// {
|
|
EnduranceTestBenchRun(&testBenchStarted_b);
|
|
|
|
if (testBenchStarted_b)
|
|
{
|
|
ProcessBoardRun(true);
|
|
HalSystemGetRunTimeMs(¤tTimeMs_u64);
|
|
|
|
if ((currentTimeMs_u64 - lastPressureUpdateMs_u64) >= 5000)
|
|
{
|
|
if (pressureSetpoint_u8 + 2 <= 15)
|
|
{
|
|
pressureSetpoint_u8 += 2;
|
|
}
|
|
else
|
|
{
|
|
pressureSetpoint_u8 = 0;
|
|
}
|
|
lastPressureUpdateMs_u64 = currentTimeMs_u64;
|
|
}
|
|
|
|
SdlEnablePressureControl(pressureSetpoint_u8);
|
|
}
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
// ProcessBoardRun(false);
|
|
// }
|
|
}
|
|
|
|
|
|
|
|
void SdlEnablePressureControl(uint8 pressureSetpoint_u8)
|
|
{
|
|
static PuControllersPidOutput_st pmpRoPressurePidOutput_st = {0};
|
|
static PuControllersPidState_st pmpRoPressurePidState_st = {0};
|
|
|
|
pmpRoPressurePidState_st.isRegulatorActive_b = true;
|
|
|
|
/* Coming from reverse model */
|
|
float32 cmdPumpRoRm_f32 = 0.0f;
|
|
pmpRoPressurePidInput_st.commandRm_f32 = cmdPumpRoRm_f32;
|
|
|
|
/* Coming from filters */
|
|
pmpRoPressurePidInput_st.measFilt_f32 = receivedFilterData_st.pressureRoPs2_f32;
|
|
pmpRoPressurePidInput_st.measFilt2_f32 = receivedFilterData_st.pmpPressureMeasFilt_f32;
|
|
|
|
pmpRoPressurePidInput_st.setpoint_f32 = pressureSetpoint_u8;
|
|
|
|
PuControllersPidLoop(&pmpRoPressurePidInput_st, &pmpRoPressurePidOutput_st, &pmpRoPressurePidState_st);
|
|
ProcessBoardGrundfosPumpHandler(pmpRoPressurePidOutput_st.commandSetpoint_f32 / 10.0f);
|
|
}
|
|
|
|
|
|
static void SdlFilterInterrupt(void)
|
|
{
|
|
receivedFilterData_st = PuFiltersRun();
|
|
}
|
|
|