diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index 0fccd9759..63543c67a 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -268,8 +268,8 @@ static void manualControlTask(void *parameters)
 
 				// Set accessory channels
 				cmd.Accessory1 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) ? scaledChannel[settings.Accessory1] : 0;
-				cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0;
-				cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0;
+				cmd.Accessory2 = (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0;
+				cmd.Accessory3 = (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0;
 
 				processFlightMode(&settings, flightMode);
 				processArm(&cmd, &settings);
diff --git a/flight/PiOS/STM32F10x/pios_delay.c b/flight/PiOS/STM32F10x/pios_delay.c
index 933fb8e7e..56c556555 100644
--- a/flight/PiOS/STM32F10x/pios_delay.c
+++ b/flight/PiOS/STM32F10x/pios_delay.c
@@ -104,6 +104,28 @@ int32_t PIOS_DELAY_WaitmS(uint16_t mS)
 	return 0;
 }
 
+/**
+ * @brief Query the Delay timer for the current uS 
+ * @return A microsecond value
+ */
+uint16_t PIOS_DELAY_GetuS()
+{
+	return PIOS_DELAY_TIMER->CNT;
+}
+
+/**
+ * @brief Compute the difference between now and a reference time
+ * @param[in] the reference time to compare now to
+ * @return The number of uS since the delay
+ * 
+ * @note the user is responsible for worrying about rollover on the 16 bit uS counter
+ */
+int32_t PIOS_DELAY_DiffuS(uint16_t ref)
+{
+	int32_t ret_t = ref;
+	return (int16_t) (PIOS_DELAY_GetuS() - ret_t);
+}
+
 #endif
 
 /**
diff --git a/flight/PiOS/inc/pios_delay.h b/flight/PiOS/inc/pios_delay.h
index 51a8755a2..647e9f553 100644
--- a/flight/PiOS/inc/pios_delay.h
+++ b/flight/PiOS/inc/pios_delay.h
@@ -36,6 +36,8 @@
 extern int32_t PIOS_DELAY_Init(void);
 extern int32_t PIOS_DELAY_WaituS(uint16_t uS);
 extern int32_t PIOS_DELAY_WaitmS(uint16_t mS);
+extern uint16_t PIOS_DELAY_GetuS();
+extern int32_t PIOS_DELAY_DiffuS(uint16_t ref);
 
 #endif /* PIOS_DELAY_H */