diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index 911d96e7d..cad5d065d 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -860,7 +860,7 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
 	FlightStatusGet(&flightStatus);
 
 	// Convert flightMode value into the switch position in the range [0..N-1]
-	uint8_t pos = (uint8_t)((flightMode + 1.0f) * settings->FlightModeNumber) >> 1;
+	uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
 	if (pos >= settings->FlightModeNumber)
 		pos = settings->FlightModeNumber - 1;
 
diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
index 550e55129..75c390643 100644
--- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
@@ -1158,8 +1158,8 @@ void ConfigInputWidget::moveFMSlider()
     m_config->fmsValue->setText(QString::number(scaledFlightMode * 100.0, 'f', 0));
 
     // Convert flightMode value into the switch position in the range [0..N-1]
-    // This should use the same logic as flight code to be consistent
-    uint8_t pos = (uint8_t)((valueScaled + 1.0) * manualSettingsDataPriv.FlightModeNumber) >> 1;
+    // This uses the same optimized computation as flight code to be consistent
+    uint8_t pos = ((int16_t)(valueScaled * 256) + 256) * manualSettingsDataPriv.FlightModeNumber >> 9;
     if (pos >= manualSettingsDataPriv.FlightModeNumber)
         pos = manualSettingsDataPriv.FlightModeNumber - 1;
     m_config->fmsSlider->setValue(pos);