diff options
author | Don Gagne <don@thegagnes.com> | 2014-04-07 18:56:03 -0700 |
---|---|---|
committer | Don Gagne <don@thegagnes.com> | 2014-04-07 18:56:03 -0700 |
commit | c1545bd237cdaf9636da55f5b907bdadc39ed130 (patch) | |
tree | 3b5900011233f0f8263be7a9b4f0a3e748a21505 /src | |
parent | f97263f5a094e9112ecdf4a5af99ad39d4a5dda4 (diff) | |
download | px4-firmware-c1545bd237cdaf9636da55f5b907bdadc39ed130.tar.gz px4-firmware-c1545bd237cdaf9636da55f5b907bdadc39ed130.tar.bz2 px4-firmware-c1545bd237cdaf9636da55f5b907bdadc39ed130.zip |
Fix float equality comparison
Also restructured incorrect return statement
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 23 |
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; |