fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 10:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix float equality comparison

Browse Source 
Also restructured incorrect return statement
This commit is contained in:
Don Gagne
2014-04-07 18:56:03 -07:00
parent f97263f5a0
commit c1545bd237
1 changed files with 12 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+12 -11
View File
@@ -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;