diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 045735d900..82312d10a9 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1304,8 +1304,15 @@ PX4IO::io_set_control_state(unsigned group)
 	for (unsigned i = 0; i < _max_controls; i++) {
 		/* ensure FLOAT_TO_REG does not produce an integer overflow */
 		const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f);
+		if(!isfinite(ctrl)){
+			regs[i] = INT16_MAX;
+		}
+		else
+		{
+			regs[i] = FLOAT_TO_REG(ctrl);
+		}
+
 
-		regs[i] = FLOAT_TO_REG(ctrl);
 	}
 
 	if (!_test_fmu_fail) {
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index dc30e3b606..bde4c35e3d 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -412,7 +412,13 @@ mixer_callback(uintptr_t handle,
 	switch (source) {
 	case MIX_FMU:
 		if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
-			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+			if(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX){
+				//catch NAN values encoded as INT16 max for disarmed outputs
+				control = NAN;
+			}
+			else{
+				control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+			}
 			break;
 		}