static void
OBD_clientParseResponse( void )
{
const uint8_t size = 8;
uint8_t sreg = CRITICAL_SECTION_ENTER();
/* Handle response */
switch ( g_OBD_rxDataBuffer[1] ) {
/* Mode 01 Response */
case OBD_MODE_01_SHOW_CURRENT_DATA: {
/* Vehicle Speed PID */
if ( g_OBD_rxDataBuffer[2] == OBD_MODE_01_PID_VEHICLE_SPEED ) {
g_OBD_vehicleSpeedKphValid = true;
g_OBD_vehicleSpeedKph = g_OBD_rxDataBuffer[3];
}
break;
}
default: {
/* do nothing */
break;
}
}
/* return to idle state */
g_OBD_state_client = OBD_STATE_IDLE;
CRITICAL_SECTION_EXIT( sreg );
}
static bool
OBD_sendPidRequest( OBD_Mode_t mode,
OBD_Pid_t pid )
{
bool success;
const uint8_t size = 8;
uint8_t sreg = CRITICAL_SECTION_ENTER();
memset( g_OBD_txDataBuffer, 0, sizeof(g_OBD_txDataBuffer) );
g_OBD_txDataBuffer[0] = (size << 4) | (OBD_FRAME_TYPE_SINGLE);
g_OBD_txDataBuffer[1] = mode;
g_OBD_txDataBuffer[2] = pid;
Log_printf( "Sending OBD request mode %u PID %u\n", mode, pid );
success = CAN_sendStandardDataFrame( OBD_11BIT_FUNC_BCAST_ADDR,
g_OBD_txDataBuffer,
sizeof(g_OBD_txDataBuffer) );
CRITICAL_SECTION_EXIT( sreg );
return success;
}
/** Get the current gains of the PID config */
void pidcfg_get_gains(pid_config_t *pid_cfg, float *kp, float *ki, float *kd)
{
CRITICAL_SECTION_ALLOC();
CRITICAL_SECTION_ENTER();
*kp = pid_cfg->kp;
*ki = pid_cfg->ki;
*kd = pid_cfg->kd;
CRITICAL_SECTION_EXIT();
}
/** Sets the PID frequency for gain compensation. */
void pidcfg_set_frequency(pid_config_t *pid_config, float frequency)
{
CRITICAL_SECTION_ALLOC();
CRITICAL_SECTION_ENTER();
pid_config->frequency = frequency;
pid_config->has_update = true;
CRITICAL_SECTION_EXIT();
}
/** Sets a maximum value for the PID integrator. */
void pidcfg_set_integral_limit(pid_config_t *pid_config, float max)
{
CRITICAL_SECTION_ALLOC();
CRITICAL_SECTION_ENTER();
pid_config->integrator_limit = max;
pid_config->has_update = true;
CRITICAL_SECTION_EXIT();
}
/** Get the current integral limit of the PID config */
float pidcfg_get_integral_limit(pid_config_t *pid_cfg)
{
float limit;
CRITICAL_SECTION_ALLOC();
CRITICAL_SECTION_ENTER();
limit = pid_cfg->integrator_limit;
CRITICAL_SECTION_EXIT();
return limit;
}
/** Sets the gains of the given PID config. */
void pidcfg_set_gains(pid_config_t *pid_config, float kp, float ki, float kd)
{
CRITICAL_SECTION_ALLOC();
CRITICAL_SECTION_ENTER();
pid_config->kp = kp;
pid_config->ki = ki;
pid_config->kd = kd;
pid_config->has_update = true;
CRITICAL_SECTION_EXIT();
}
/** Get the current frequency of the PID config */
float pidcfg_get_frequency(pid_config_t *pid_cfg)
{
float freq;
CRITICAL_SECTION_ALLOC();
CRITICAL_SECTION_ENTER();
freq = pid_cfg->frequency;
CRITICAL_SECTION_EXIT();
return freq;
}
static void
OBD_serverRespond( void )
{
const uint8_t size = 8;
uint8_t sreg = CRITICAL_SECTION_ENTER();
memset( g_OBD_txDataBuffer, 0, sizeof(g_OBD_txDataBuffer) );
/* Handle requests */
switch ( g_OBD_rxDataBuffer[1] ) {
/* Mode 01 Requests */
case OBD_MODE_01_SHOW_CURRENT_DATA: {
/* Vehicle Speed PID */
if ( g_OBD_rxDataBuffer[2] == OBD_MODE_01_PID_VEHICLE_SPEED ) {
/* TODO: Check data byte 1 for correct response */
g_OBD_txDataBuffer[0] = (size << 4) | (OBD_FRAME_TYPE_SINGLE);
g_OBD_txDataBuffer[1] = OBD_MODE_01_SHOW_CURRENT_DATA;
g_OBD_txDataBuffer[2] = OBD_MODE_01_PID_VEHICLE_SPEED;
g_OBD_txDataBuffer[3] = 123; /* arbitrary speed (0-255) */
/* Send response to diagnostic tool address */
CAN_sendStandardDataFrame( OBD_11BIT_DIAG_TOOL_ADDR,
g_OBD_txDataBuffer,
sizeof(g_OBD_txDataBuffer) );
}
break;
}
default: {
/* do nothing */
break;
}
}
/* return to idle state */
g_OBD_state_server = OBD_STATE_IDLE;
CRITICAL_SECTION_EXIT( sreg );
}
/** Transfer configuration to PID (thread-safe) */
void pidcfg_apply(pid_config_t *cfg)
{
CRITICAL_SECTION_ALLOC();
// check if something to apply
if (!cfg->has_update) {
return;
}
CRITICAL_SECTION_ENTER();
pid_set_gains(cfg->target_pid, cfg->kp, cfg->ki, cfg->kd);
pid_set_integral_limit(cfg->target_pid, cfg->integrator_limit);
pid_set_frequency(cfg->target_pid, cfg->frequency);
cfg->has_update = false;
CRITICAL_SECTION_EXIT();
}