From 316fab88006633050a2762e3760dc95513a5a4c8 Mon Sep 17 00:00:00 2001
From: rusefi <arro239+rusefi@gmail.com>
Date: Sun, 25 Feb 2018 21:17:59 -0500
Subject: [PATCH] #569

---
 firmware/hw_layer/neo6m.cpp       |  2 +-
 firmware/hw_layer/servo.cpp       | 46 +++++++++++++++++++++++++++++++
 firmware/hw_layer/servo.h         |  1 +
 firmware/tunerstudio/rusefi.input |  1 +
 4 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/firmware/hw_layer/neo6m.cpp b/firmware/hw_layer/neo6m.cpp
index f8845540d2..ed03b36ce9 100644
--- a/firmware/hw_layer/neo6m.cpp
+++ b/firmware/hw_layer/neo6m.cpp
@@ -107,7 +107,7 @@ static bool isGpsEnabled() {
 }
 
 void initGps(void) {
-	if(!isGpsEnabled())
+	if (!isGpsEnabled())
 		return;
 
 
diff --git a/firmware/hw_layer/servo.cpp b/firmware/hw_layer/servo.cpp
index 32db651e72..fb75f3198c 100644
--- a/firmware/hw_layer/servo.cpp
+++ b/firmware/hw_layer/servo.cpp
@@ -9,4 +9,50 @@
 
 #include "servo.h"
 #include "pin_repository.h"
+#include "engine.h"
+
+// todo: remove this hack once we have next incompatible configuration change
+#define TEMP_FOR_COMPATIBILITY GPIOA_0
+
+EXTERN_ENGINE;
+
+THD_WORKING_AREA(seThreadStack, UTILITY_THREAD_STACK_SIZE);
+
+OutputPin pin;
+
+static int countServos() {
+	int result = 0;
+
+	for (int i = 0; i < SERVO_COUNT; i++) {
+		if (engineConfiguration->servoOutputPins[i] != GPIO_UNASSIGNED &&
+				engineConfiguration->servoOutputPins[i] != TEMP_FOR_COMPATIBILITY) {
+			result++;
+		}
+	}
+	return result;
+
+}
+
+static msg_t seThread(void *arg) {
+	(void)arg;
+	chRegSetThreadName("servo");
+	while (true) {
+		int count = countServos();
+
+		chThdSleepMilliseconds(19 - 2 * count);
+	}
+	return 0;
+}
+
+void initServo(void) {
+
+
+	brain_pin_e p = engineConfiguration->servoOutputPins[0];
+	if (p != TEMP_FOR_COMPATIBILITY) {
+		pin.initPin("servo", p);
+	}
+
+	chThdCreateStatic(seThreadStack, sizeof(seThreadStack), NORMALPRIO, (tfunc_t) seThread, NULL);
+}
+
 
diff --git a/firmware/hw_layer/servo.h b/firmware/hw_layer/servo.h
index bce1618e64..d6439385bd 100644
--- a/firmware/hw_layer/servo.h
+++ b/firmware/hw_layer/servo.h
@@ -9,5 +9,6 @@
 
 #include "main.h"
 
+void initServo(void);
 
 #endif /* HW_LAYER_SERVO_H_ */
diff --git a/firmware/tunerstudio/rusefi.input b/firmware/tunerstudio/rusefi.input
index f4ed69bb1b..c0595d6b95 100644
--- a/firmware/tunerstudio/rusefi.input
+++ b/firmware/tunerstudio/rusefi.input
@@ -1299,6 +1299,7 @@ cmd_stop_engine              = "w\x00\x99\x00\x00"
 		field = "clutchDownPin",						clutchDownPin
 		field = "clutchUpPin",							clutchUpPin
 		field = "brakePedalPin",						brakePedalPin
+		field = "servo#1",								servoOutputPins1
 
 	dialog = allPins2
 		field = "Tachometer output Pin",					tachOutputPin