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(); }