diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 97acf8f10a..67739deb83 100644 --- a/APMrover2/APMrover2.cpp +++ b/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()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); + g2.motors.set_throttle(g.throttle_min.get()); + 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()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in()); + g2.motors.set_throttle(channel_throttle->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); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); + g2.motors.set_throttle(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()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); + g2.motors.set_throttle(g.throttle_min.get()); + g2.motors.set_steering(0); lateral_acceleration = 0; } else { calc_lateral_acceleration(); diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 9e3bb307cc..47132ce234 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/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); } diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index d0848e3ef2..7c68179404 100644 --- a/APMrover2/Log.cpp +++ b/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)); diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index aa29a5398e..9321145e42 100644 --- a/APMrover2/Parameters.cpp +++ b/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 }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index bcfe0b5ed3..0787166b9a 100644 --- a/APMrover2/Parameters.h +++ b/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[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index b4176beae6..7bc3804393 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -63,6 +63,7 @@ #include #include #include +#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); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index a4d8188c47..271913f513 100644 --- a/APMrover2/Steering.cpp +++ b/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()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); + if (g2.motors.have_skid_steering()) { + 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)); -} - -/* - 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); + g2.motors.set_steering(steerController.get_steering_out_lat_accel(lateral_acceleration)); } /***************************************** 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 - throttle_slew_limit(); - } - // Apply skid steering mixing - if (have_skid_steering()) { - mix_skid_steering(); + g2.motors.slew_limit_throttle(true); + } else { + g2.motors.slew_limit_throttle(false); } - if (!arming.is_armed()) { - // Some ESCs get noisy (beep error msgs) if PWM == 0. - // 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; + // send output signals to motors + g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt); } diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 55468c9ca9..0ee559f2f7 100644 --- a/APMrover2/commands_logic.cpp +++ b/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()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); + g2.motors.set_throttle(g.throttle_min.get()); + 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()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); + g2.motors.set_throttle(g.throttle_min.get()); + 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; diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index edda9f248c..8e371823c0 100644 --- a/APMrover2/control_modes.cpp +++ b/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; } } diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index 5c7563e926..df703e908b 100644 --- a/APMrover2/crash_check.cpp +++ b/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; } diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 8857b17dec..3fa1869d0a 100644 --- a/APMrover2/radio.cpp +++ b/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) { - 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); - } + if (!arming.is_armed()) { + g2.motors.setup_safety_output(); } // 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()); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in()); + g2.motors.set_throttle(channel_throttle->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) && - (((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) && in_reverse) || - ((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) && !in_reverse))) { + if ((fabs(g2.motors.get_throttle()) > 50) && + (((g2.motors.get_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 { diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 0d16c6cf88..60789141d1 100644 --- a/APMrover2/system.cpp +++ b/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