AP_MotorsUGV: add motor output test

H2CO3 · Jul 18, 2017 · 2cd3dd5 · 2cd3dd5
1 parent 8421575
commit 2cd3dd5
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 0 deletions.
diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp
@@ -312,3 +312,52 @@ void AP_MotorsUGV::setup_safety_output()
     SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
     SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
 }
+
+bool AP_MotorsUGV::output_test(motor_test_order motor_seq)
+{
+    // check if the motor_seq is valid
+    if (motor_seq > THROTTLE_RIGHT) {
+        return false;
+    }
+    _throttle = constrain_float(_throttle, -100.0f, 100.0f);
+
+    switch (motor_seq) {
+        case THROTTLE: {
+            if (!SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
+                return false;
+            }
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, _throttle);
+            break;
+        }
+        case STEERING: {
+            if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
+                return false;
+            }
+            SRV_Channels::set_output_scaled(SRV_Channel::k_steering, _throttle);
+            break;
+        }
+        case THROTTLE_LEFT: {
+            if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
+                return false;
+            }
+            const float motorLeft = brushed_scaler((_throttle * 0.01f), _throttleLeft_servo);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motorLeft);
+            break;
+        }
+        case THROTTLE_RIGHT: {
+            if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
+                return false;
+            }
+            const float motorRight = brushed_scaler((_throttle * 0.01f), _throttleRight_servo);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motorRight);
+            break;
+        }
+        default:
+            return false;
+    }
+    SRV_Channels::calc_pwm();
+    hal.rcout->cork();
+    SRV_Channels::output_ch_all();
+    hal.rcout->push();
+    return true;
+}
diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h
@@ -18,6 +18,13 @@ class AP_MotorsUGV {
         PWM_TYPE_BRUSHEDBIPOLAR = 4,
      };
 
+    enum motor_test_order {
+        THROTTLE = 0,
+        STEERING = 1,
+        THROTTLE_LEFT = 2,
+        THROTTLE_RIGHT = 3,
+    };
+
     // initialise motors
     void init();
 
@@ -43,6 +50,8 @@ class AP_MotorsUGV {
     // set when to use slew rate limiter
     void slew_limit_throttle(bool value) { _use_slew_rate = value; }
 
+    bool output_test(motor_test_order motor_seq);
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];