From d620efbcb7cfdece6fa25427a2cab8f5243055ca Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 25 Aug 2016 20:16:54 +0900 Subject: [PATCH] Rover: if statements is simple. --- APMrover2/control_modes.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index d243516156..46f84794dd 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -51,12 +51,12 @@ void Rover::read_control_switch() uint8_t Rover::readSwitch(void){ uint16_t pulsewidth = hal.rcin->read(g.mode_channel - 1); if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition - if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; - if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; - if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; - if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual - if (pulsewidth >= 1750) return 5; // Hardware Manual - return 0; + if (pulsewidth <= 1230) return 0; + if (pulsewidth <= 1360) return 1; + if (pulsewidth <= 1490) return 2; + if (pulsewidth <= 1620) return 3; + if (pulsewidth <= 1749) return 4; // Software Manual + return 5; // Hardware Manual } void Rover::reset_control_switch()