From 93d6b012c2615780ed46140934f05af3c6908182 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Jan 2017 21:31:10 +1100 Subject: [PATCH] Rover: convert to new SRV_Channel API --- APMrover2/APMrover2.cpp | 27 +++++++------ APMrover2/GCS_Mavlink.cpp | 4 +- APMrover2/Log.cpp | 8 ++-- APMrover2/Parameters.cpp | 69 ++++++-------------------------- APMrover2/Parameters.h | 68 ++++++++++---------------------- APMrover2/Rover.h | 1 + APMrover2/Steering.cpp | 76 +++++++++++++++++------------------- APMrover2/commands_logic.cpp | 6 +-- APMrover2/control_modes.cpp | 2 +- APMrover2/crash_check.cpp | 2 +- APMrover2/failsafe.cpp | 2 +- APMrover2/radio.cpp | 36 ++++++++--------- APMrover2/system.cpp | 9 ----- APMrover2/test.cpp | 66 ------------------------------- 14 files changed, 112 insertions(+), 264 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 72bf125425..03f93c8c54 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -300,7 +300,7 @@ void Rover::update_logging2(void) */ void Rover::update_aux(void) { - RC_Channel_aux::enable_aux_servos(); + SRV_Channels::enable_aux_servos(); } /* @@ -452,17 +452,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 (channel_throttle->get_servo_out() != g.throttle_min.get()) { + if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) { gcs_send_mission_item_reached_message(0); } - channel_throttle->set_servo_out(g.throttle_min.get()); - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); lateral_acceleration = 0; } else { calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); - Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt), Vector3f(g.speed_cruise, channel_throttle->get_servo_out(), 0)); + Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt), + Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0)); } break; @@ -510,18 +511,18 @@ void Rover::update_current_mode(void) we set the exact value in set_servos(), but it helps for logging */ - channel_throttle->set_servo_out(channel_throttle->get_control_in()); - channel_steer->set_servo_out(channel_steer->pwm_to_angle()); + 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->pwm_to_angle()); // mark us as in_reverse when using a negative throttle to // stop AHRS getting off - set_reverse(channel_throttle->get_servo_out() < 0); + set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0); break; case HOLD: // hold position - stop motors and center steering - channel_throttle->set_servo_out(0); - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); break; case INITIALISING: @@ -548,7 +549,7 @@ void Rover::update_navigation() calc_lateral_acceleration(); calc_nav_steer(); if (verify_RTL()) { - channel_throttle->set_servo_out(g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); set_mode(HOLD); } break; @@ -565,8 +566,8 @@ void Rover::update_navigation() calc_nav_steer(); if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are - channel_throttle->set_servo_out(g.throttle_min.get()); - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); lateral_acceleration = 0; } break; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 19bd9df669..e266f1a005 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, - 10000 * channel_throttle->norm_output(), + 10000 * SRV_Channels::get_output_norm(SRV_Channel::k_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, - (uint16_t)(100 * fabsf(channel_throttle->norm_output())), + (uint16_t)(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))), current_loc.alt / 100.0, 0); } diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index e05c423ab5..1869b0ef91 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -238,10 +238,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)channel_steer->get_servo_out(), + steer_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_steering), roll : (int16_t)ahrs.roll_sensor, pitch : (int16_t)ahrs.pitch_sensor, - throttle_out : (int16_t)channel_throttle->get_servo_out(), + throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -268,7 +268,7 @@ void Rover::Log_Write_Nav_Tuning() wp_distance : wp_distance, target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(), nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(), - throttle : (int8_t)(100 * channel_throttle->norm_output()), + throttle : (int8_t)(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle)), xtrack_error : nav_controller->crosstrack_error() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -326,7 +326,7 @@ void Rover::Log_Write_Sonar() turn_angle : (int8_t)obstacle.turn_angle, turn_time : turn_time, ground_speed : (uint16_t)(ground_speed*100), - throttle : (int8_t)(100 * channel_throttle->norm_output()) + throttle : (int8_t)(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle)) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index e350be47e8..707420a1fe 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -174,62 +174,6 @@ const AP_Param::Info Rover::var_info[] = { // @User: Standard GSCALAR(ch7_option, "CH7_OPTION", CH7_OPTION), - // @Group: RC1_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_1, "RC1_", RC_Channel), - - // @Group: RC2_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_2, "RC2_", RC_Channel_aux), - - // @Group: RC3_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp - GGROUP(rc_3, "RC3_", RC_Channel), - - // @Group: RC4_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_4, "RC4_", RC_Channel_aux), - - // @Group: RC5_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_5, "RC5_", RC_Channel_aux), - - // @Group: RC6_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_6, "RC6_", RC_Channel_aux), - - // @Group: RC7_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_7, "RC7_", RC_Channel_aux), - - // @Group: RC8_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_8, "RC8_", RC_Channel_aux), - - // @Group: RC9_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_9, "RC9_", RC_Channel_aux), - - // @Group: RC10_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_10, "RC10_", RC_Channel_aux), - - // @Group: RC11_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_11, "RC11_", RC_Channel_aux), - - // @Group: RC12_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_12, "RC12_", RC_Channel_aux), - - // @Group: RC13_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_13, "RC13_", RC_Channel_aux), - - // @Group: RC14_ - // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp - GGROUP(rc_14, "RC14_", RC_Channel_aux), - // @Param: THR_MIN // @DisplayName: Minimum Throttle // @Description: The minimum throttle setting to which the autopilot will apply. This is mostly useful for rovers with internal combustion motors, to prevent the motor from cutting out in auto mode. @@ -585,10 +529,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("SYSID_ENFORCE", 2, ParametersG2, sysid_enforce, 0), + // @Group: SERVO + // @Path: ../libraries/SRV_Channel/SRV_Channel.cpp + AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels), + + // @Group: RC + // @Path: ../libraries/RC_Channel/RC_Channel.cpp + AP_SUBGROUPINFO(rc_channels, "RC", 4, ParametersG2, RC_Channels), + AP_GROUPEND }; +ParametersG2::ParametersG2(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + /* This is a conversion table from old parameter values to new diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 19ec647f15..42742db513 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -33,8 +33,8 @@ public: k_param_relay, k_param_BoardConfig, k_param_pivot_turn_angle, - k_param_rc_13, - k_param_rc_14, + k_param_rc_13_old, + k_param_rc_14_old, // IO pins k_param_rssi_pin = 20, // unused, replaced by rssi_ library parameters @@ -113,14 +113,14 @@ public: // // 160: Radio settings // - k_param_rc_1 = 160, - k_param_rc_2, - k_param_rc_3, - k_param_rc_4, - k_param_rc_5, - k_param_rc_6, - k_param_rc_7, - k_param_rc_8, + k_param_rc_1_old = 160, + k_param_rc_2_old, + k_param_rc_3_old, + k_param_rc_4_old, + k_param_rc_5_old, + k_param_rc_6_old, + k_param_rc_7_old, + k_param_rc_8_old, // throttle control k_param_throttle_min = 170, @@ -182,10 +182,10 @@ public: k_param_pidSpeedThrottle, // high RC channels - k_param_rc_9 = 235, - k_param_rc_10, - k_param_rc_11, - k_param_rc_12, + k_param_rc_9_old = 235, + k_param_rc_10_old, + k_param_rc_11_old, + k_param_rc_12_old, // other objects k_param_sitl = 240, @@ -242,22 +242,6 @@ public: AP_Int16 pivot_turn_angle; AP_Int16 gcs_pid_mask; - // RC channels - RC_Channel rc_1; - RC_Channel_aux rc_2; - RC_Channel rc_3; - RC_Channel_aux rc_4; - RC_Channel_aux rc_5; - RC_Channel_aux rc_6; - RC_Channel_aux rc_7; - RC_Channel_aux rc_8; - RC_Channel_aux rc_9; - RC_Channel_aux rc_10; - RC_Channel_aux rc_11; - RC_Channel_aux rc_12; - RC_Channel_aux rc_13; - RC_Channel_aux rc_14; - // Throttle // AP_Int8 throttle_min; @@ -302,22 +286,6 @@ public: PID pidSpeedThrottle; Parameters() : - // RC channels - rc_1(CH_1), - rc_2(CH_2), - rc_3(CH_3), - rc_4(CH_4), - rc_5(CH_5), - rc_6(CH_6), - rc_7(CH_7), - rc_8(CH_8), - rc_9(CH_9), - rc_10(CH_10), - rc_11(CH_11), - rc_12(CH_12), - rc_13(CH_13), - rc_14(CH_14), - // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- pidSpeedThrottle (0.7, 0.2, 0.2, 4000) @@ -329,7 +297,7 @@ public: */ class ParametersG2 { public: - ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); } + ParametersG2(void); // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -339,6 +307,12 @@ public: // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; + + // RC input channels + RC_Channels rc_channels; + + // control over servo output ranges + SRV_Channels servo_channels; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6703a637e0..36e5c35b3f 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -92,6 +92,7 @@ class Rover : public AP_HAL::HAL::Callbacks { public: friend class GCS_MAVLINK_Rover; friend class Parameters; + friend class ParametersG2; friend class AP_Arming; Rover(void); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 018e53ca00..f40b925e69 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -12,7 +12,10 @@ void Rover::throttle_slew_limit(int16_t last_throttle) { if (temp < 1) { temp = 1; } - channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp)); + uint16_t pwm; + if (SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, pwm)) { + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,constrain_int16(pwm, last_throttle - temp, last_throttle + temp)); + } } } @@ -100,10 +103,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()) { - channel_throttle->set_servo_out(g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); // Stop rotation in case of loitering and skid steering if (g.skid_steer_out) { - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); } return; } @@ -146,9 +149,9 @@ void Rover::calc_throttle(float target_speed) { throttle *= reduction; if (in_reverse) { - channel_throttle->set_servo_out(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); } else { - channel_throttle->set_servo_out(constrain_int16(throttle, g.throttle_min, g.throttle_max)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(throttle, g.throttle_min, g.throttle_max)); } if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) { @@ -161,7 +164,7 @@ void Rover::calc_throttle(float target_speed) { // is 2*braking_speederr float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1); int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain; - channel_throttle->set_servo_out(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); // temporarily set us in reverse to allow the PWM setting to // go negative @@ -169,7 +172,7 @@ void Rover::calc_throttle(float target_speed) { } if (use_pivot_steering()) { - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); } } @@ -219,7 +222,7 @@ void Rover::calc_lateral_acceleration() { void Rover::calc_nav_steer() { // check to see if the rover is loitering if (in_stationary_loiter()) { - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); return; } @@ -231,68 +234,61 @@ 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); - channel_steer->set_servo_out(steerController.get_steering_out_lat_accel(lateral_acceleration)); + SRV_Channels::set_output_scaled(SRV_Channel::k_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) { - static int16_t last_throttle; - - // support a separate steering channel - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0)); + static uint16_t last_throttle; if (control_mode == MANUAL || control_mode == LEARNING) { // do a direct pass through of radio values - channel_steer->set_radio_out(channel_steer->read()); - channel_throttle->set_radio_out(channel_throttle->read()); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->read()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->read()); if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { // suppress throttle if in failsafe and manual - channel_throttle->set_radio_out(channel_throttle->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->get_radio_trim()); // suppress steer if in failsafe and manual and skid steer mode if (g.skid_steer_out) { - channel_steer->set_radio_out(channel_steer->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->get_radio_trim()); } } } else { - channel_steer->calc_pwm(); if (in_reverse) { - channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), + 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 { - channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), g.throttle_min.get(), g.throttle_max.get())); } if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) { // suppress throttle if in failsafe - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); // suppress steer if in failsafe and skid steer mode if (g.skid_steer_out) { - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); } } if (!hal.util->get_soft_armed()) { - channel_throttle->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); // suppress steer if in failsafe and skid steer mode if (g.skid_steer_out) { - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); } } - // convert 0 to 100% into PWM - channel_throttle->calc_pwm(); - // limit throttle movement speed throttle_slew_limit(last_throttle); } // record last throttle before we apply skid steering - last_throttle = channel_throttle->get_radio_out(); + SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle); if (g.skid_steer_out) { // convert the two radio_out values to skid steering values @@ -303,14 +299,12 @@ void Rover::set_servos(void) { motor1 = throttle + 0.5*steering motor2 = throttle - 0.5*steering */ - float steering_scaled = channel_steer->norm_output(); - float throttle_scaled = channel_throttle->norm_output(); + float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering); + float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle); float motor1 = throttle_scaled + 0.5f*steering_scaled; float motor2 = throttle_scaled - 0.5f*steering_scaled; - channel_steer->set_servo_out(4500*motor1); - channel_throttle->set_servo_out(100*motor2); - channel_steer->calc_pwm(); - channel_throttle->calc_pwm(); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,4500*motor1); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,100*motor2); } if (!arming.is_armed()) { @@ -323,28 +317,28 @@ void Rover::set_servos(void) { break; case AP_Arming::YES_ZERO_PWM: - channel_throttle->set_radio_out(0); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,0); if (g.skid_steer_out) { - channel_steer->set_radio_out(0); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering,0); } break; case AP_Arming::YES_MIN_PWM: default: - channel_throttle->set_radio_out(channel_throttle->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->get_radio_trim()); if (g.skid_steer_out) { - channel_steer->set_radio_out(channel_steer->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->get_radio_trim()); } break; } } + SRV_Channels::calc_pwm(); + #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS // send values to the PWM timers for output // ---------------------------------------- - channel_steer->output(); - channel_throttle->output(); - RC_Channel_aux::output_ch_all(); + SRV_Channels::output_ch_all(); #endif } diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 822892c4a2..1874a44dda 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -370,14 +370,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_yaw_speed.msg_time_ms) > 3000) { gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping"); - channel_throttle->set_servo_out(g.throttle_min.get()); - channel_steer->set_servo_out(0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); lateral_acceleration = 0; return; } int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle); - channel_steer->set_servo_out(steering); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering,steering); // speed param in the message gives speed as a proportion of cruise speed. // 0.5 would set speed to the cruise speed diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index 43771151d4..a9925f881c 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -140,7 +140,7 @@ bool Rover::motor_active() { // Check if armed and throttle is not neutral if (hal.util->get_soft_armed()) { - if (channel_throttle->get_servo_out() != channel_throttle->get_radio_trim()) { + if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != channel_throttle->get_radio_trim()) { return true; } } diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index e532829a07..b8f15fdc40 100644 --- a/APMrover2/crash_check.cpp +++ b/APMrover2/crash_check.cpp @@ -21,7 +21,7 @@ void Rover::crash_check() // TODO : Check if min vel can be calculated // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; - if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN)|| ((100 * fabsf(channel_throttle->norm_output())) < CRASH_CHECK_THROTTLE_MIN)) { + if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN)|| ((100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))) < CRASH_CHECK_THROTTLE_MIN)) { crash_counter = 0; return; } diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index 0001f0ddf9..b749bc984e 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -46,7 +46,7 @@ void Rover::failsafe_check() for (uint8_t ch=start_ch; ch < 4; ch++) { hal.rcout->write(ch, hal.rcin->read(ch)); } - RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); + SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true); } } diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 58bc9707d7..c18c20914f 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -5,14 +5,20 @@ */ void Rover::set_control_channels(void) { - channel_steer = RC_Channel::rc_channel(rcmap.roll()-1); - channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); - channel_learn = RC_Channel::rc_channel(g.learn_channel-1); + 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); + SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering); + SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle); + // set rc channel ranges 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); + // For a rover safety is TRIM throttle if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim()); @@ -37,20 +43,10 @@ void Rover::init_rc_in() void Rover::init_rc_out() { - RC_Channel::rc_channel(CH_1)->enable_out(); - RC_Channel::rc_channel(CH_3)->enable_out(); - - if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) { - channel_throttle->enable_out(); - if (g.skid_steer_out) { - channel_steer->enable_out(); - } - } - - RC_Channel::output_trim_all(); + SRV_Channels::output_trim_all(); // setup PWM values to send if the FMU firmware dies - RC_Channel::setup_failsafe_trim_all(); + 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. @@ -134,17 +130,17 @@ void Rover::read_radio() failsafe.last_valid_rc_ms = AP_HAL::millis(); - RC_Channel::set_pwm_all(); + RC_Channels::set_pwm_all(); control_failsafe(channel_throttle->get_radio_in()); - channel_throttle->set_servo_out(channel_throttle->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->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(channel_throttle->get_servo_out()) > 50) && - (((channel_throttle->get_servo_out() < 0) && in_reverse) || - ((channel_throttle->get_servo_out() > 0) && !in_reverse))) { + 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))) { 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 190280e169..b4c8301826 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -510,9 +510,6 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method) return false; } - // only log if arming was successful - channel_throttle->enable_out(); - change_arm_state(); return true; } @@ -525,12 +522,6 @@ bool Rover::disarm_motors(void) if (!arming.disarm()) { return false; } - if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) { - channel_throttle->disable_out(); - if (g.skid_steer_out) { - channel_steer->disable_out(); - } - } if (control_mode != AUTO) { // reset the mission on disarm if we are not in auto mission.reset(); diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index 81e07bfc92..7c1ecce0b9 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -7,8 +7,6 @@ // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details static const struct Menu::command test_menu_commands[] = { - {"pwm", MENU_FUNC(test_radio_pwm)}, - {"radio", MENU_FUNC(test_radio)}, {"passthru", MENU_FUNC(test_passthru)}, {"failsafe", MENU_FUNC(test_failsafe)}, {"relay", MENU_FUNC(test_relay)}, @@ -42,35 +40,6 @@ void Rover::print_hit_enter() cliSerial->printf("Hit Enter to exit.\n\n"); } -int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while (1) { - delay(20); - - // Filters radio input - adjust filters in the radio.cpp file - // ---------------------------------------------------------- - read_radio(); - - cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - channel_steer->get_radio_in(), - g.rc_2.get_radio_in(), - channel_throttle->get_radio_in(), - g.rc_4.get_radio_in(), - g.rc_5.get_radio_in(), - g.rc_6.get_radio_in(), - g.rc_7.get_radio_in(), - g.rc_8.get_radio_in()); - - if (cliSerial->available() > 0) { - return (0); - } - } -} - - int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); @@ -96,41 +65,6 @@ int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv) return 0; } -int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - // read the radio to set trims - // --------------------------- - trim_radio(); - - while (1) { - delay(20); - read_radio(); - - channel_steer->calc_pwm(); - channel_throttle->calc_pwm(); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - channel_steer->get_control_in(), - g.rc_2.get_control_in(), - channel_throttle->get_control_in(), - g.rc_4.get_control_in(), - g.rc_5.get_control_in(), - g.rc_6.get_control_in(), - g.rc_7.get_control_in(), - g.rc_8.get_control_in()); - - if (cliSerial->available() > 0) { - return (0); - } - } -} int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) {