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++;