#569

firmware/hw_layer/neo6m.cpp

@@ -107,7 +107,7 @@ static bool isGpsEnabled() {
 }
 
 void initGps(void) {
-	if(!isGpsEnabled())
+	if (!isGpsEnabled())
 		return;
 
 

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

firmware/hw_layer/servo.h

@@ -9,5 +9,6 @@
 
 #include "main.h"
 
+void initServo(void);
 
 #endif /* HW_LAYER_SERVO_H_ */

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