diff --git a/firmware/controllers/algo/algo.mk b/firmware/controllers/algo/algo.mk
index 3286f41911..9644f671e9 100644
--- a/firmware/controllers/algo/algo.mk
+++ b/firmware/controllers/algo/algo.mk
@@ -2,11 +2,11 @@
 CONTROLLERS_ALGO_SRC = $(PROJECT_DIR)/controllers/algo/map_adjuster.c \
 	$(PROJECT_DIR)/controllers/algo/signal_executor.c \
 	$(PROJECT_DIR)/controllers/algo/malfunction_central.c \
-	$(PROJECT_DIR)/controllers/algo/idle_controller.c \
 	$(PROJECT_DIR)/controllers/algo/nmea.c
 	
 CONTROLLERS_ALGO_SRC_CPP = $(PROJECT_DIR)/controllers/algo/OutputSignalArray.cpp \
 	$(PROJECT_DIR)/controllers/algo/advance_map.cpp \
+	$(PROJECT_DIR)/controllers/algo/idle_controller.cpp \
 	$(PROJECT_DIR)/controllers/algo/auto_generated_enums.cpp \
 	$(PROJECT_DIR)/controllers/algo/fuel_math.cpp \
 	$(PROJECT_DIR)/controllers/algo/accel_enrichment.cpp \
diff --git a/firmware/controllers/algo/idle_controller.c b/firmware/controllers/algo/idle_controller.cpp
similarity index 94%
rename from firmware/controllers/algo/idle_controller.c
rename to firmware/controllers/algo/idle_controller.cpp
index 65b1e92b51..9390360433 100644
--- a/firmware/controllers/algo/idle_controller.c
+++ b/firmware/controllers/algo/idle_controller.cpp
@@ -1,5 +1,5 @@
 /**
- * @file	idle_controller.c
+ * @file	idle_controller.cpp
  * @brief	Simple Idle Air Valve control algorithm
  *
  *			This algorithm is trying to get current RPM to the desired 'target' value
@@ -13,11 +13,10 @@
  * @author Andrey Belomutskiy, (c) 2012-2014
  */
 
+#include "main.h"
 #include "idle_controller.h"
 #include "efilib.h"
-
-// todo: move this to "idle_controller.h"
-int isCranking(void);
+#include "rpm_calculator.h"
 
 static int lastGoodValue = DEFAULT_IDLE_DUTY;
 
@@ -35,7 +34,7 @@ void setIdleRpm(IdleValveState *idle, int targetRpm) {
 /**
  * @brief	sets new idle valve duty cycle: checks the bounds and reports new value
  */
-static int setNewValue(IdleValveState *idle, int currentRpm, int now, char * msg, int newValue) {
+static int setNewValue(IdleValveState *idle, int currentRpm, int now, const char * msg, int newValue) {
 	newValue = maxI(newValue, MIN_IDLE);
 	newValue = minI(newValue, MAX_IDLE);
 
@@ -48,7 +47,7 @@ static int setNewValue(IdleValveState *idle, int currentRpm, int now, char * msg
 	return newValue;
 }
 
-static int changeValue(IdleValveState *idle, int currentRpm, int now, char * msg, int delta) {
+static int changeValue(IdleValveState *idle, int currentRpm, int now, const char * msg, int delta) {
 	int newValue = idle->value + delta;
 	return setNewValue(idle, currentRpm, now, msg, newValue);
 }
diff --git a/firmware/controllers/algo/idle_controller.h b/firmware/controllers/algo/idle_controller.h
index b26ae30318..a5910f2048 100644
--- a/firmware/controllers/algo/idle_controller.h
+++ b/firmware/controllers/algo/idle_controller.h
@@ -22,26 +22,27 @@
 #define IDLE_INCREASE_STEP 5
 #define IDLE_DECREASE_STEP 5
 
-
-typedef struct {
+class IdleValveState {
+public:
 	int time;
 
 	int targetRpmRangeLeft, targetRpmRangeRight;
 
 	int value;
 	int timeOfLastIdleChange;
-} IdleValveState;
+};
+
+void idleInit(IdleValveState *idle);
+int getIdle(IdleValveState *idle, int currentRpm, int time);
+void setIdleRpm(IdleValveState *idle, int targetRpm);
+
+void idleDebug(const char *msg, int value);
 
 #ifdef __cplusplus
 extern "C"
 {
 #endif /* __cplusplus */
 
-void idleInit(IdleValveState *idle);
-int getIdle(IdleValveState *idle, int currentRpm, int time);
-void setIdleRpm(IdleValveState *idle, int targetRpm);
-
-void idleDebug(char *msg, int value);
 
 #ifdef __cplusplus
 }
diff --git a/firmware/controllers/engine_controller.cpp b/firmware/controllers/engine_controller.cpp
index aa311a1971..c71a1a0bfd 100644
--- a/firmware/controllers/engine_controller.cpp
+++ b/firmware/controllers/engine_controller.cpp
@@ -398,7 +398,7 @@ void initEngineContoller(Engine *engine) {
 
 #if EFI_IDLE_CONTROL
 	if (engineConfiguration->isIdleThreadEnabled) {
-		startIdleThread();
+		startIdleThread(engine);
 	}
 #else
 	scheduleMsg(&logger, "no idle control");
diff --git a/firmware/controllers/idle_thread.cpp b/firmware/controllers/idle_thread.cpp
index bf7b8e3a63..da1451a321 100644
--- a/firmware/controllers/idle_thread.cpp
+++ b/firmware/controllers/idle_thread.cpp
@@ -56,12 +56,12 @@ int getIdleSwitch() {
 	return idleSwitchState;
 }
 
-void idleDebug(char *msg, int value) {
+void idleDebug(const char *msg, int value) {
 	printMsg(&logger, "%s%d", msg, value);
 	scheduleLogging(&logger);
 }
 
-static void setIdleControlEnabled(int value) {
+static void setIdleControlEnabled(int value, Engine *engine) {
 	engineConfiguration->idleMode = value ? IM_MANUAL : IM_AUTO;
 	scheduleMsg(&logger, "isIdleControlActive=%d", engineConfiguration->idleMode);
 }
@@ -113,7 +113,7 @@ static void setIdleRpmAction(int value) {
 	scheduleMsg(&logger, "target idle RPM %d", value);
 }
 
-void startIdleThread() {
+void startIdleThread(Engine *engine) {
 	initLogging(&logger, "Idle Valve Control");
 
 	/**
@@ -136,5 +136,5 @@ void startIdleThread() {
 
 	addConsoleActionI("set_idle_rpm", setIdleRpmAction);
 	addConsoleActionI("set_idle_pwm", setIdleValvePwm);
-	addConsoleActionI("set_idle_enabled", setIdleControlEnabled);
+	addConsoleActionIP("set_idle_enabled", (VoidIntVoidPtr)setIdleControlEnabled, engine);
 }
diff --git a/firmware/controllers/idle_thread.h b/firmware/controllers/idle_thread.h
index 699307af55..2c92245f67 100644
--- a/firmware/controllers/idle_thread.h
+++ b/firmware/controllers/idle_thread.h
@@ -9,7 +9,9 @@
 #ifndef IDLE_THREAD_H_
 #define IDLE_THREAD_H_
 
-void startIdleThread(void);
+#include "engine.h"
+
+void startIdleThread(Engine *engine);
 int getIdleSwitch(void);
 
 #endif /* IDLE_THREAD_H_ */
diff --git a/firmware/util/cli_registry.c b/firmware/util/cli_registry.c
index b8068798b3..760b08d71e 100644
--- a/firmware/util/cli_registry.c
+++ b/firmware/util/cli_registry.c
@@ -119,6 +119,7 @@ static int getParameterCount(action_type_e parameterType) {
 	case NO_PARAMETER_P:
 		return 0;
 	case ONE_PARAMETER:
+	case ONE_PARAMETER_P:
 	case FLOAT_PARAMETER:
 	case STRING_PARAMETER:
 		return 1;
@@ -345,10 +346,16 @@ void handleActionWithParameter(TokenCallback *current, char *parameter) {
 		return;
 	}
 
-	VoidInt callback1 = (VoidInt) current->callback;
+	if (current->parameterType == ONE_PARAMETER_P) {
+		VoidIntVoidPtr callback1 = (VoidIntVoidPtr) current->callback;
+		// invoke callback function by reference
+		(*callback1)(value, current->param);
 
-	// invoke callback function by reference
-	(*callback1)(value);
+	} else {
+		VoidInt callback1 = (VoidInt) current->callback;
+		// invoke callback function by reference
+		(*callback1)(value);
+	}
 
 }