Rover: integrate motors library

move throttle_slew_limit and THR_SLEWRATE parameter
move have_skid_steering to library
move mix_skid_steering to library's output_skid_steering method
move radio.cpp's output channel initialisation to motor's init method
use motors.get_throttle and get_steering instead of getting from Servo objects
use motors.set_throttle and set_steering instead of setting to Servo object
AP_Arming::arming_required is replaced with SAFE_DISARM parameter

APMrover2/APMrover2.cpp

@@ -438,18 +438,18 @@ void Rover::update_current_mode(void)
         case Guided_WP:
             if (rtl_complete || verify_RTL()) {
                 // we have reached destination so stop where we are
-                if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) {
+                if (fabsf(g2.motors.get_throttle()) > g.throttle_min.get()) {
                     gcs_send_mission_item_reached_message(0);
                 }
-                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
+                g2.motors.set_throttle(g.throttle_min.get());
-                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+                g2.motors.set_steering(0);
                 lateral_acceleration = 0;
             } else {
                 calc_lateral_acceleration();
                 calc_nav_steer();
                 calc_throttle(rover.guided_control.target_speed);
                 Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
-                                       Vector3f(rover.guided_control.target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0.0f));
+                                       Vector3f(rover.guided_control.target_speed, g2.motors.get_throttle(), 0.0f));
             }
             break;
 
@@ -501,18 +501,18 @@ void Rover::update_current_mode(void)
           we set the exact value in set_servos(), but it helps for
           logging
          */
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
+        g2.motors.set_throttle(channel_throttle->get_control_in());
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in());
+        g2.motors.set_steering(channel_steer->get_control_in());
 
         // mark us as in_reverse when using a negative throttle to
         // stop AHRS getting off
-        set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0);
+        set_reverse(is_negative(g2.motors.get_throttle()));
         break;
 
     case HOLD:
         // hold position - stop motors and center steering
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
+        g2.motors.set_throttle(0);
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+        g2.motors.set_steering(0);
         if (!in_auto_reverse) {
             set_reverse(false);
         }
@@ -543,7 +543,7 @@ void Rover::update_navigation()
     case RTL:
         // no loitering around the wp with the rover, goes direct to the wp position
         if (verify_RTL()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
+            g2.motors.set_throttle(g.throttle_min.get());
             set_mode(HOLD);
         } else {
             calc_lateral_acceleration();
@@ -561,8 +561,8 @@ void Rover::update_navigation()
             // no loitering around the wp with the rover, goes direct to the wp position
             if (rtl_complete || verify_RTL()) {
                 // we have reached destination so stop where we are
-                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
+                g2.motors.set_throttle(g.throttle_min.get());
-                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+                g2.motors.set_steering(0);
                 lateral_acceleration = 0;
             } else {
                 calc_lateral_acceleration();

APMrover2/GCS_Mavlink.cpp

@@ -162,7 +162,7 @@ void Rover::send_servo_out(mavlink_channel_t chan)
         0,  // port 0
         10000 * channel_steer->norm_output(),
         0,
-        100 * SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
+        g2.motors.get_throttle(),
         0,
         0,
         0,
@@ -179,7 +179,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan)
         gps.ground_speed(),
         ahrs.groundspeed(),
         (ahrs.yaw_sensor / 100) % 360,
-        SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
+        g2.motors.get_throttle(),
         current_loc.alt / 100.0f,
         0);
 }

APMrover2/Log.cpp

@@ -248,10 +248,10 @@ void Rover::Log_Write_Control_Tuning()
     struct log_Control_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
         time_us         : AP_HAL::micros64(),
-        steer_out       : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_steering),
+        steer_out       : (int16_t)g2.motors.get_steering(),
         roll            : (int16_t)ahrs.roll_sensor,
         pitch           : (int16_t)ahrs.pitch_sensor,
-        throttle_out    : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
+        throttle_out    : (int16_t)g2.motors.get_throttle(),
         accel_y         : accel.y
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));

APMrover2/Parameters.cpp

@@ -202,15 +202,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(throttle_cruise,        "CRUISE_THROTTLE",    50),
 
-    // @Param: THR_SLEWRATE
-    // @DisplayName: Throttle slew rate
-    // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. A value of 100 means the throttle can change over its full range in one second. Note that for some NiMH powered rovers setting a lower value like 40 or 50 may be worthwhile as the sudden current demand on the battery of a big rise in throttle may cause a brownout.
-    // @Units: %/s
-    // @Range: 0 100
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(throttle_slewrate,      "THR_SLEWRATE",     100),
-
     // @Param: SKID_STEER_IN
     // @DisplayName: Skid steering input
     // @Description: Set this to 1 for skid steering input rovers (tank track style in RC controller). When enabled, servo1 is used for the left track control, servo3 is used for right track control
@@ -551,6 +542,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp
     AP_SUBGROUPINFO(visual_odom, "VISO", 7, ParametersG2, AP_VisualOdom),
 
+    // @Group: MOT_
+    // @Path: MotorsUGV.cpp
+    AP_SUBGROUPINFO(motors, "MOT_", 8, ParametersG2, AP_MotorsUGV),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -127,7 +127,7 @@ public:
         k_param_throttle_min = 170,
         k_param_throttle_max,
         k_param_throttle_cruise,
-        k_param_throttle_slewrate,
+        k_param_throttle_slewrate_old,
         k_param_throttle_reduction,
         k_param_skid_steer_in,
         k_param_skid_steer_out_old,
@@ -248,7 +248,6 @@ public:
     AP_Int8     throttle_min;
     AP_Int8     throttle_max;
     AP_Int8     throttle_cruise;
-    AP_Int8     throttle_slewrate;
     AP_Int8     skid_steer_in;
 
     // failsafe control
@@ -322,6 +321,9 @@ public:
 
     // Visual Odometry camera
     AP_VisualOdom visual_odom;
+
+    // Motor library
+    AP_MotorsUGV motors;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -63,6 +63,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_BoardConfig/AP_BoardConfig_CAN.h>
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
+#include "AP_MotorsUGV.h"
 
 #include "AP_Arming.h"
 #include "compat.h"
@@ -478,14 +479,11 @@ private:
     void Log_Arm_Disarm();
 
     void load_parameters(void);
-    void throttle_slew_limit(void);
     bool auto_check_trigger(void);
     bool use_pivot_steering(void);
     void calc_throttle(float target_speed);
     void calc_lateral_acceleration();
     void calc_nav_steer();
-    bool have_skid_steering();
-    void mix_skid_steering();
     void set_servos(void);
     void set_auto_WP(const struct Location& loc);
     void set_guided_WP(const struct Location& loc);

APMrover2/Steering.cpp

@@ -1,18 +1,5 @@
 #include "Rover.h"
 
-/*****************************************
-    Throttle slew limit
-*****************************************/
-void Rover::throttle_slew_limit(void) {
-    if (g.throttle_slewrate > 0) {
-        SRV_Channels::limit_slew_rate(SRV_Channel::k_throttle, g.throttle_slewrate, G_Dt);
-        if (have_skid_steering()) {
-            // when skid steering also limit 2nd channel
-            SRV_Channels::limit_slew_rate(SRV_Channel::k_steering, g.throttle_slewrate, G_Dt);
-        }
-    }
-}
-
 /*
     check for triggering of start of auto mode
 */
@@ -65,7 +52,7 @@ bool Rover::auto_check_trigger(void) {
 bool Rover::use_pivot_steering(void)
 {
     // check cases where we clearly cannot use pivot steering
-    if (control_mode < AUTO || !have_skid_steering() || g.pivot_turn_angle <= 0) {
+    if (control_mode < AUTO || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
         pivot_steering_active = false;
         return false;
     }
@@ -114,10 +101,10 @@ void Rover::calc_throttle(float target_speed) {
     // If not autostarting OR we are loitering at a waypoint
     // then set the throttle to minimum
     if (!auto_check_trigger() || in_stationary_loiter()) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
+        g2.motors.set_throttle(g.throttle_min.get());
         // Stop rotation in case of loitering and skid steering
-        if (have_skid_steering()) {
+        if (g2.motors.have_skid_steering()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+            g2.motors.set_steering(0);
         }
         return;
     }
@@ -160,9 +147,9 @@ void Rover::calc_throttle(float target_speed) {
     throttle *= reduction;
 
     if (in_reverse) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
+        g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
     } else {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(throttle, g.throttle_min, g.throttle_max));
+        g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
     }
 
     if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
@@ -175,7 +162,7 @@ void Rover::calc_throttle(float target_speed) {
         // is 2*braking_speederr
         const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
         const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
+        g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
 
         // temporarily set us in reverse to allow the PWM setting to
         // go negative
@@ -185,7 +172,7 @@ void Rover::calc_throttle(float target_speed) {
     if (guided_mode != Guided_Velocity) {
         if (use_pivot_steering()) {
             // In Guided Velocity, only the steering input is used to calculate the pivot turn.
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
+            g2.motors.set_throttle(0);
         }
     }
 }
@@ -236,7 +223,7 @@ void Rover::calc_lateral_acceleration() {
 void Rover::calc_nav_steer() {
     // check to see if the rover is loitering
     if (in_stationary_loiter()) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+        g2.motors.set_steering(0);
         return;
     }
 
@@ -248,142 +235,20 @@ void Rover::calc_nav_steer() {
     // constrain to max G force
     lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
 
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steerController.get_steering_out_lat_accel(lateral_acceleration));
+    g2.motors.set_steering(steerController.get_steering_out_lat_accel(lateral_acceleration));
-}
-
-/*
-  run the skid steering mixer
- */
-void Rover::mix_skid_steering(void)
-{
-    float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f;         // steering scaled -1 to +1
-    float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f;    // throttle scaled -1 to +1
-
-    // apply constraints
-    steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f);
-    throttle_scaled = constrain_float(throttle_scaled, -1.0f, 1.0f);
-
-    // check for saturation and scale back throttle and steering proportionally
-    const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled);
-    if (saturation_value > 1.0f) {
-        steering_scaled = steering_scaled / saturation_value;
-        throttle_scaled = throttle_scaled / saturation_value;
-    }
-
-    // add in throttle
-    float motor_left = throttle_scaled;
-    float motor_right = throttle_scaled;
-
-    // deal with case of turning on the spot
-    if (is_zero(throttle_scaled)) {
-        // full possible range is not used to keep response equivalent to non-zero throttle case
-        motor_left += steering_scaled * 0.5f;
-        motor_right -= steering_scaled * 0.5f;
-    } else {
-        // add in steering
-        const float dir = is_positive(throttle_scaled) ? 1.0f : -1.0f;
-        if (is_negative(steering_scaled)) {
-            // moving left all steering to right wheel
-            motor_right -= dir * steering_scaled;
-        } else {
-            // turning right, all steering to left wheel
-            motor_left += dir * steering_scaled;
-        }
-    }
-
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft,  1000.0f * motor_left);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor_right);
 }
 
 /*****************************************
     Set the flight control servos based on the current calculated values
 *****************************************/
 void Rover::set_servos(void) {
-    if (in_reverse) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-                                                                                 -g.throttle_max,
-                                                                                 -g.throttle_min));
-    } else {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-                                                                                 g.throttle_min,
-                                                                                 g.throttle_max));
-    }
-    // Check Throttle failsafe in non auto mode. Suppress all ouput
-    if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
-        // suppress throttle if in failsafe
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-        // suppress steer if in failsafe and skid steer mode
-        if (have_skid_steering()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
-        }
-    }
-    // Check if soft arm. Suppress all ouput
-    if (!hal.util->get_soft_armed()) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-        // suppress steer if in failsafe and skid steer mode
-        if (have_skid_steering()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
-        }
-    }
     // Apply slew rate limit on non Manual modes
     if (control_mode != MANUAL && control_mode != LEARNING) {
-        // limit throttle movement speed
+        g2.motors.slew_limit_throttle(true);
-        throttle_slew_limit();
+    } else {
-    }
+        g2.motors.slew_limit_throttle(false);
-    // Apply skid steering mixing
-    if (have_skid_steering()) {
-        mix_skid_steering();
     }
 
-    if (!arming.is_armed()) {
+    // send output signals to motors
-        // Some ESCs get noisy (beep error msgs) if PWM == 0.
+    g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt);
-        // This little segment aims to avoid this.
-        switch (arming.arming_required()) {
-        case AP_Arming::NO:
-            // keep existing behavior: do nothing to radio_out
-            // (don't disarm throttle channel even if AP_Arming class is)
-            break;
-
-        case AP_Arming::YES_ZERO_PWM:
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
-            if (have_skid_steering()) {
-                SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
-            }
-            break;
-
-        case AP_Arming::YES_MIN_PWM:
-        default:
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-            if (have_skid_steering()) {
-                SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-            }
-            break;
-        }
-    }
-
-    SRV_Channels::calc_pwm();
-
-#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
-    // send values to the PWM timers for output
-    // ----------------------------------------
-    hal.rcout->cork();
-    SRV_Channels::output_ch_all();
-    hal.rcout->push();
-#endif
-}
-
-/*
-  work out if skid steering is available
- */
-bool Rover::have_skid_steering(void)
-{
-    if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) &&
-        SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
-        return true;
-    }
-    return false;
 }

APMrover2/commands_logic.cpp

@@ -377,14 +377,14 @@ void Rover::nav_set_yaw_speed()
     // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
     if ((millis() - guided_control.msg_time_ms) > 3000) {
         gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
+        g2.motors.set_throttle(g.throttle_min.get());
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+        g2.motors.set_steering(0);
         lateral_acceleration = 0.0f;
         return;
     }
 
     const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
+    g2.motors.set_steering(steering);
 
     // speed param in the message gives speed as a proportion of cruise speed.
     // 0.5 would set speed to the cruise speed
@@ -400,8 +400,8 @@ void Rover::nav_set_speed()
     // if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED message within the last 3 seconds bring the rover to a halt
     if ((millis() - guided_control.msg_time_ms) > 3000) {
         gcs_send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
+        g2.motors.set_throttle(g.throttle_min.get());
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
+        g2.motors.set_steering(0);
         lateral_acceleration = 0.0f;
         prev_WP = current_loc;
         next_WP = current_loc;
@@ -415,7 +415,7 @@ void Rover::nav_set_speed()
     location_update(next_WP, (steer_value + ahrs.yaw_sensor) * 0.01f, 4.0f);  // put the next wp at 4m forward at steer direction
     nav_controller->update_waypoint(current_loc, next_WP);
 
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
+    g2.motors.set_steering(steer_value);
     calc_throttle(guided_control.target_speed);
 
     Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
@@ -459,7 +459,7 @@ void Rover::do_yaw(const AP_Mission::Mission_Command& cmd)
 
     // Calculate the steering to apply base on error calculated
     const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
+    g2.motors.set_steering(steering);
     next_navigation_leg_cd = condition_value;
     calc_throttle(g.speed_cruise);
 
@@ -473,16 +473,16 @@ bool Rover::do_yaw_rotation()
 
     // check if we are within 5 degrees of the target heading
     if (error_to_target_yaw <= 500) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);  // stop the current rotation
+        g2.motors.set_steering(0);  // stop the current rotation
         condition_value = condition_start;  // reset the condition value to its previous value
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
+        g2.motors.set_throttle(0);
         next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
         do_auto_rotation = false;
         return true;
     } else {
         // Calculate the steering to apply base on error calculated
         const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
+        g2.motors.set_steering(steering);
         calc_throttle(g.speed_cruise);
         do_auto_rotation = true;
         return false;

APMrover2/control_modes.cpp

@@ -140,7 +140,7 @@ bool Rover::motor_active()
 {
     // Check if armed and output throttle servo is not neutral
     if (hal.util->get_soft_armed()) {
-        if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) {
+        if (!is_zero(g2.motors.get_throttle())) {
             return true;
         }
     }

APMrover2/crash_check.cpp

@@ -23,7 +23,7 @@ void Rover::crash_check()
 
   if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) ||        // Check velocity
       (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) ||  // Check turn speed
-      (fabsf(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) < CRASH_CHECK_THROTTLE_MIN)) {
+      (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN)) {
     crash_counter = 0;
     return;
   }

APMrover2/radio.cpp

@@ -5,6 +5,7 @@
  */
 void Rover::set_control_channels(void)
 {
+    // check change on RCMAP
     channel_steer    = RC_Channels::rc_channel(rcmap.roll()-1);
     channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
     channel_learn    = RC_Channels::rc_channel(g.learn_channel-1);
@@ -13,19 +14,9 @@ void Rover::set_control_channels(void)
     channel_steer->set_angle(SERVO_MAX);
     channel_throttle->set_angle(100);
 
-    SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
-    SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
-
-    // left/right throttle as -1000 to 1000 values
-    SRV_Channels::set_angle(SRV_Channel::k_throttleLeft,  1000);
-    SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
-
     // For a rover safety is TRIM throttle
-    if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
+    if (!arming.is_armed()) {
-        SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
+        g2.motors.setup_safety_output();
-        if (have_skid_steering()) {
-            SRV_Channels::set_safety_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-        }
     }
     // setup correct scaling for ESCs like the UAVCAN PX4ESC which
     // take a proportion of speed. Default to 1000 to 2000 for systems without
@@ -50,17 +41,6 @@ void Rover::init_rc_out()
 
     // setup PWM values to send if the FMU firmware dies
     SRV_Channels::setup_failsafe_trim_all();
-
-    // output throttle trim when safety off if arming
-    // is setup for min on disarm.  MIN is from plane where MIN is effectively no throttle.
-    // For Rover's no throttle means TRIM as rovers can go backwards i.e. MIN throttle is
-    // full speed backward.
-    if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
-        SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-        if (have_skid_steering()) {
-            SRV_Channels::set_safety_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-        }
-    }
 }
 
 /*
@@ -100,7 +80,7 @@ void Rover::rudder_arm_disarm_check()
             // not at full right rudder
             rudder_arm_timer = 0;
         }
-    } else if (!motor_active() & !have_skid_steering()) {
+    } else if (!motor_active() & !g2.motors.have_skid_steering()) {
         // when armed and motor not active (not moving), full left rudder starts disarming counter
         // This is disabled for skid steering otherwise when tring to turn a skid steering rover around
         // the rover would disarm
@@ -138,14 +118,14 @@ void Rover::read_radio()
     // check that RC value are valid
     control_failsafe(channel_throttle->get_radio_in());
     // copy RC scaled inputs to outputs
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
+    g2.motors.set_throttle(channel_throttle->get_control_in());
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in());
+    g2.motors.set_steering(channel_steer->get_control_in());
 
     // Check if the throttle value is above 50% and we need to nudge
     // Make sure its above 50% in the direction we are travelling
-    if ((abs(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) > 50) &&
+    if ((fabs(g2.motors.get_throttle()) > 50) &&
-        (((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) && in_reverse) ||
+        (((g2.motors.get_throttle() < 0) && in_reverse) ||
-         ((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) && !in_reverse))) {
+         ((g2.motors.get_throttle() > 0) && !in_reverse))) {
             throttle_nudge = (g.throttle_max - g.throttle_cruise) *
                 ((fabsf(channel_throttle->norm_input()) - 0.5f) / 0.5f);
     } else {

APMrover2/system.cpp

@@ -180,6 +180,9 @@ void Rover::init_ardupilot()
     init_rc_in();        // sets up rc channels from radio
     init_rc_out();        // sets up the timer libs
 
+    // init motors including setting rc out channels ranges
+    g2.motors.init();
+
     relay.init();
 
 #if MOUNT == ENABLED