diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index b5ee5fb86e..1384146076 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -356,7 +356,7 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) { // check if the motor_seq is valid - if (motor_seq > MOTOR_TEST_THROTTLE_RIGHT) { + if (motor_seq >= MOTOR_TEST_LAST) { return false; } pct = constrain_float(pct, -100.0f, 100.0f); @@ -404,7 +404,7 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) } break; } - default: + case MOTOR_TEST_LAST: return false; } SRV_Channels::calc_pwm(); diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h index 300249a1f9..3bab650d14 100644 --- a/APMrover2/AP_MotorsUGV.h +++ b/APMrover2/AP_MotorsUGV.h @@ -23,7 +23,8 @@ public: MOTOR_TEST_STEERING = 2, MOTOR_TEST_THROTTLE_LEFT = 3, MOTOR_TEST_THROTTLE_RIGHT = 4, - MOTOR_TEST_MAINSAIL = 5 + MOTOR_TEST_MAINSAIL = 5, + MOTOR_TEST_LAST }; // supported custom motor configurations