Fix float equality comparison

Also restructured incorrect return statement

1 files changed, 12 insertions, 11 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1c11e2587..57faafcfd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -473,18 +473,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-            // Follow exactly what the mavlink spec says for values
-            if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) {
-                return VEHICLE_CMD_RESULT_UNSUPPORTED;
-            }
-        
-            transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command");
-            if (arming_res == TRANSITION_DENIED) {
-                mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-                result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+            // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+            // We use an float epsilon delta to test float equality.
+            if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
             } else {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-			}
+                transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+                if (arming_res == TRANSITION_DENIED) {
+                    mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+                    result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+                } else {
+                    result = VEHICLE_CMD_RESULT_ACCEPTED;
+                }
+            }
 		}
 		break;