From 8c9e55edfa73d8f9ca677e53dde87e780fd3617d Mon Sep 17 00:00:00 2001 From: skyscraper Date: Sun, 8 May 2016 09:49:39 +0100 Subject: [PATCH] APMRover2: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step) --- APMrover2/APMrover2.cpp | 22 ++++++++--------- APMrover2/GCS_Mavlink.cpp | 16 ++++++------- APMrover2/Log.cpp | 6 ++--- APMrover2/Steering.cpp | 48 ++++++++++++++++++------------------- APMrover2/control_modes.cpp | 4 ++-- APMrover2/radio.cpp | 42 ++++++++++++++++---------------- APMrover2/test.cpp | 40 +++++++++++++++---------------- 7 files changed, 89 insertions(+), 89 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index edc095f2e4..4134a10105 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -420,11 +420,11 @@ void Rover::update_current_mode(void) set_reverse(false); if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are - if (channel_throttle->servo_out != g.throttle_min.get()) { + if (channel_throttle->get_servo_out() != g.throttle_min.get()) { gcs_send_mission_item_reached_message(0); } - channel_throttle->servo_out = g.throttle_min.get(); - channel_steer->servo_out = 0; + channel_throttle->set_servo_out(g.throttle_min.get()); + channel_steer->set_servo_out(0); lateral_acceleration = 0; } else { calc_lateral_acceleration(); @@ -470,18 +470,18 @@ void Rover::update_current_mode(void) we set the exact value in set_servos(), but it helps for logging */ - channel_throttle->servo_out = channel_throttle->control_in; - channel_steer->servo_out = channel_steer->pwm_to_angle(); + channel_throttle->set_servo_out(channel_throttle->get_control_in()); + channel_steer->set_servo_out(channel_steer->pwm_to_angle()); // mark us as in_reverse when using a negative throttle to // stop AHRS getting off - set_reverse(channel_throttle->servo_out < 0); + set_reverse(channel_throttle->get_servo_out() < 0); break; case HOLD: // hold position - stop motors and center steering - channel_throttle->servo_out = 0; - channel_steer->servo_out = 0; + channel_throttle->set_servo_out(0); + channel_steer->set_servo_out(0); set_reverse(false); break; @@ -509,7 +509,7 @@ void Rover::update_navigation() calc_lateral_acceleration(); calc_nav_steer(); if (verify_RTL()) { - channel_throttle->servo_out = g.throttle_min.get(); + channel_throttle->set_servo_out(g.throttle_min.get()); set_mode(HOLD); } break; @@ -520,8 +520,8 @@ void Rover::update_navigation() calc_nav_steer(); if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are - channel_throttle->servo_out = g.throttle_min.get(); - channel_steer->servo_out = 0; + channel_throttle->set_servo_out(g.throttle_min.get()); + channel_steer->set_servo_out(0); lateral_acceleration = 0; } break; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 7880d74b91..0d632c0bec 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -280,14 +280,14 @@ void Rover::send_radio_out(mavlink_channel_t chan) chan, micros(), 0, // port - RC_Channel::rc_channel(0)->radio_out, - RC_Channel::rc_channel(1)->radio_out, - RC_Channel::rc_channel(2)->radio_out, - RC_Channel::rc_channel(3)->radio_out, - RC_Channel::rc_channel(4)->radio_out, - RC_Channel::rc_channel(5)->radio_out, - RC_Channel::rc_channel(6)->radio_out, - RC_Channel::rc_channel(7)->radio_out); + RC_Channel::rc_channel(0)->get_radio_out(), + RC_Channel::rc_channel(1)->get_radio_out(), + RC_Channel::rc_channel(2)->get_radio_out(), + RC_Channel::rc_channel(3)->get_radio_out(), + RC_Channel::rc_channel(4)->get_radio_out(), + RC_Channel::rc_channel(5)->get_radio_out(), + RC_Channel::rc_channel(6)->get_radio_out(), + RC_Channel::rc_channel(7)->get_radio_out()); #endif } diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 07940ac486..92627b88f2 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -240,10 +240,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->servo_out, + steer_out : (int16_t)channel_steer->get_servo_out(), roll : (int16_t)ahrs.roll_sensor, pitch : (int16_t)ahrs.pitch_sensor, - throttle_out : (int16_t)channel_throttle->servo_out, + throttle_out : (int16_t)channel_throttle->get_servo_out(), accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -333,7 +333,7 @@ void Rover::Log_Write_Sonar() void Rover::Log_Write_Current() { - DataFlash.Log_Write_Current(battery, channel_throttle->control_in); + DataFlash.Log_Write_Current(battery, channel_throttle->get_control_in()); // also write power status DataFlash.Log_Write_Power(); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index ff2b37729f..38f8299530 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -9,12 +9,12 @@ void Rover::throttle_slew_limit(int16_t last_throttle) { // if slew limit rate is set to zero then do not slew limit if (g.throttle_slewrate && last_throttle != 0) { // limit throttle change by the given percentage per second - float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min); + float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min()); // allow a minimum change of 1 PWM per cycle if (temp < 1) { temp = 1; } - channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp); + channel_throttle->set_radio_out (constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp)); } } @@ -85,7 +85,7 @@ 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() || ((loiter_time > 0) && (control_mode == AUTO))) { - channel_throttle->servo_out = g.throttle_min.get(); + channel_throttle->set_servo_out(g.throttle_min.get()); return; } @@ -127,9 +127,9 @@ void Rover::calc_throttle(float target_speed) { throttle *= reduction; if (in_reverse) { - channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min); + channel_throttle->set_servo_out(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); } else { - channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max); + channel_throttle->set_servo_out(constrain_int16(throttle, g.throttle_min, g.throttle_max)); } if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) { @@ -142,7 +142,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->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min); + channel_throttle->set_servo_out(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); // temporarily set us in reverse to allow the PWM setting to // go negative @@ -150,7 +150,7 @@ void Rover::calc_throttle(float target_speed) { } if (use_pivot_steering()) { - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); } } @@ -194,7 +194,7 @@ void Rover::calc_lateral_acceleration() { void Rover::calc_nav_steer() { // check to see if the rover is loitering if ((loiter_time > 0) && (control_mode == AUTO)) { - channel_steer->servo_out = 0; + channel_steer->set_servo_out(0); return; } @@ -204,7 +204,7 @@ 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->servo_out = steerController.get_steering_out_lat_accel(lateral_acceleration); + channel_steer->set_servo_out(steerController.get_steering_out_lat_accel(lateral_acceleration)); } /***************************************** @@ -214,35 +214,35 @@ void Rover::set_servos(void) { static int16_t last_throttle; // support a separate steering channel - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0)); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0)); if (control_mode == MANUAL || control_mode == LEARNING) { // do a direct pass through of radio values - channel_steer->radio_out = channel_steer->read(); - channel_throttle->radio_out = channel_throttle->read(); + channel_steer->set_radio_out(channel_steer->read()); + channel_throttle->set_radio_out(channel_throttle->read()); if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { // suppress throttle if in failsafe and manual - channel_throttle->radio_out = channel_throttle->radio_trim; + channel_throttle->set_radio_out(channel_throttle->get_radio_trim()); } } else { channel_steer->calc_pwm(); if (in_reverse) { - channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, + channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), -g.throttle_max, - -g.throttle_min); + -g.throttle_min)); } else { - channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, + channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), g.throttle_min.get(), - g.throttle_max.get()); + g.throttle_max.get())); } if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) { // suppress throttle if in failsafe - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); } if (!hal.util->get_soft_armed()) { - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); } // convert 0 to 100% into PWM @@ -253,7 +253,7 @@ void Rover::set_servos(void) { } // record last throttle before we apply skid steering - last_throttle = channel_throttle->radio_out; + last_throttle = channel_throttle->get_radio_out(); if (g.skid_steer_out) { // convert the two radio_out values to skid steering values @@ -268,8 +268,8 @@ void Rover::set_servos(void) { float throttle_scaled = channel_throttle->norm_output(); float motor1 = throttle_scaled + 0.5f*steering_scaled; float motor2 = throttle_scaled - 0.5f*steering_scaled; - channel_steer->servo_out = 4500*motor1; - channel_throttle->servo_out = 100*motor2; + channel_steer->set_servo_out(4500*motor1); + channel_throttle->set_servo_out(100*motor2); channel_steer->calc_pwm(); channel_throttle->calc_pwm(); } @@ -284,12 +284,12 @@ void Rover::set_servos(void) { break; case AP_Arming::YES_ZERO_PWM: - channel_throttle->radio_out = 0; + channel_throttle->set_radio_out(0); break; case AP_Arming::YES_MIN_PWM: default: - channel_throttle->radio_out = channel_throttle->radio_trim; + channel_throttle->set_radio_out(channel_throttle->get_radio_trim()); break; } } diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index 8c85ccbd64..d243516156 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -75,7 +75,7 @@ void Rover::read_trim_switch() case CH7_DO_NOTHING: break; case CH7_SAVE_WP: - if (channel_learn->radio_in > CH_7_PWM_TRIGGER) { + if (channel_learn->get_radio_in() > CH_7_PWM_TRIGGER) { // switch is engaged ch7_flag = true; } else { // switch is disengaged @@ -86,7 +86,7 @@ void Rover::read_trim_switch() hal.console->println("Erasing waypoints"); // if SW7 is ON in MANUAL = Erase the Flight Plan mission.clear(); - if (channel_steer->control_in > 3000) { + if (channel_steer->get_control_in() > 3000) { // if roll is full right store the current location as home init_home(); } diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 7e2abb975f..cf7df427b6 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -17,12 +17,12 @@ void Rover::set_control_channels(void) // 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->radio_trim); + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim()); } // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. - hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max); + hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); } void Rover::init_rc_in() @@ -54,7 +54,7 @@ void Rover::init_rc_out() // 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) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_trim); + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim()); } } @@ -78,7 +78,7 @@ void Rover::rudder_arm_disarm_check() if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter - if (channel_steer->control_in > 4000) { + if (channel_steer->get_control_in() > 4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || @@ -98,7 +98,7 @@ void Rover::rudder_arm_disarm_check() } } else if (!motor_active()) { // when armed and motor not active (not moving), full left rudder starts disarming counter - if (channel_steer->control_in < -4000) { + if (channel_steer->get_control_in() < -4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || @@ -121,7 +121,7 @@ void Rover::rudder_arm_disarm_check() void Rover::read_radio() { if (!hal.rcin->new_input()) { - control_failsafe(channel_throttle->radio_in); + control_failsafe(channel_throttle->get_radio_in()); return; } @@ -129,15 +129,15 @@ void Rover::read_radio() RC_Channel::set_pwm_all(); - control_failsafe(channel_throttle->radio_in); + control_failsafe(channel_throttle->get_radio_in()); - channel_throttle->servo_out = channel_throttle->control_in; + channel_throttle->set_servo_out(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->servo_out) > 50) && - (((channel_throttle->servo_out < 0) && in_reverse) || - ((channel_throttle->servo_out > 0) && !in_reverse))) { + if ((abs(channel_throttle->get_servo_out()) > 50) && + (((channel_throttle->get_servo_out() < 0) && in_reverse) || + ((channel_throttle->get_servo_out() > 0) && !in_reverse))) { throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f); } else { @@ -158,17 +158,17 @@ void Rover::read_radio() float motor2 = channel_throttle->norm_input(); float steering_scaled = motor1 - motor2; float throttle_scaled = 0.5f*(motor1 + motor2); - int16_t steer = channel_steer->radio_trim; - int16_t thr = channel_throttle->radio_trim; + int16_t steer = channel_steer->get_radio_trim(); + int16_t thr = channel_throttle->get_radio_trim(); if (steering_scaled > 0.0f) { - steer += steering_scaled*(channel_steer->radio_max-channel_steer->radio_trim); + steer += steering_scaled*(channel_steer->get_radio_max()-channel_steer->get_radio_trim()); } else { - steer += steering_scaled*(channel_steer->radio_trim-channel_steer->radio_min); + steer += steering_scaled*(channel_steer->get_radio_trim()-channel_steer->get_radio_min()); } if (throttle_scaled > 0.0f) { - thr += throttle_scaled*(channel_throttle->radio_max-channel_throttle->radio_trim); + thr += throttle_scaled*(channel_throttle->get_radio_max()-channel_throttle->get_radio_trim()); } else { - thr += throttle_scaled*(channel_throttle->radio_trim-channel_throttle->radio_min); + thr += throttle_scaled*(channel_throttle->get_radio_trim()-channel_throttle->get_radio_min()); } channel_steer->set_pwm(steer); channel_throttle->set_pwm(thr); @@ -211,9 +211,9 @@ bool Rover::throttle_failsafe_active(void) return true; } if (channel_throttle->get_reverse()) { - return channel_throttle->radio_in >= g.fs_throttle_value; + return channel_throttle->get_radio_in() >= g.fs_throttle_value; } - return channel_throttle->radio_in <= g.fs_throttle_value; + return channel_throttle->get_radio_in() <= g.fs_throttle_value; } void Rover::trim_control_surfaces() @@ -221,8 +221,8 @@ void Rover::trim_control_surfaces() read_radio(); // Store control surface trim values // --------------------------------- - if (channel_steer->radio_in > 1400) { - channel_steer->radio_trim = channel_steer->radio_in; + if (channel_steer->get_radio_in() > 1400) { + channel_steer->set_radio_trim(channel_steer->get_radio_in()); // save to eeprom channel_steer->save_eeprom(); } diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index bca42065bc..6f97b0167c 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -57,14 +57,14 @@ int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv) read_radio(); cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - channel_steer->radio_in, - g.rc_2.radio_in, - channel_throttle->radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in); + 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); @@ -119,14 +119,14 @@ int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv) set_servos(); cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", - channel_steer->control_in, - g.rc_2.control_in, - channel_throttle->control_in, - g.rc_4.control_in, - g.rc_5.control_in, - g.rc_6.control_in, - g.rc_7.control_in, - g.rc_8.control_in); + 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); @@ -150,7 +150,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) oldSwitchPosition = readSwitch(); cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n"); - while(channel_throttle->control_in > 0){ + while(channel_throttle->get_control_in() > 0){ delay(20); read_radio(); } @@ -159,8 +159,8 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) delay(20); read_radio(); - if(channel_throttle->control_in > 0){ - cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->control_in); + if(channel_throttle->get_control_in() > 0){ + cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->get_control_in()); fail_test++; } @@ -172,7 +172,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) } if(throttle_failsafe_active()) { - cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->radio_in); + cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in()); print_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++;