diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 52c3cedffa..7376d11017 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -518,7 +518,7 @@ void Plane::handle_auto_mode(void) if (auto_state.land_complete) { // we are in the final stage of a landing - force // zero throttle - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); } } else { // we are doing normal AUTO flight, the special cases @@ -638,7 +638,7 @@ void Plane::update_flight_mode(void) // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); } if (g.fbwa_tdrag_chan > 0) { // check for the user enabling FBWA taildrag takeoff mode @@ -667,7 +667,7 @@ void Plane::update_flight_mode(void) roll when heading is locked. Heading becomes unlocked on any aileron or rudder input */ - if ((channel_roll->control_in != 0 || + if ((channel_roll->get_control_in() != 0 || rudder_input != 0)) { cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; @@ -703,8 +703,8 @@ void Plane::update_flight_mode(void) case MANUAL: // servo_out is for Sim control only // --------------------------------- - channel_roll->servo_out = channel_roll->pwm_to_angle(); - channel_pitch->servo_out = channel_pitch->pwm_to_angle(); + channel_roll->set_servo_out(channel_roll->pwm_to_angle()); + channel_pitch->set_servo_out(channel_pitch->pwm_to_angle()); steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 @@ -928,7 +928,7 @@ void Plane::update_flight_stage(void) set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - if ((g.land_abort_throttle_enable && channel_throttle->control_in >= 90) || + if ((g.land_abort_throttle_enable && channel_throttle->get_control_in() >= 90) || auto_state.commanded_go_around || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){ // abort mode is sticky, it must complete while executing NAV_LAND diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index a05f3c395c..d0162a752b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -21,8 +21,8 @@ float Plane::get_speed_scaler(void) } speed_scaler = constrain_float(speed_scaler, 0.5f, 2.0f); } else { - if (channel_throttle->servo_out > 0) { - speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->servo_out / 2.0f); // First order taylor expansion of square root + if (channel_throttle->get_servo_out() > 0) { + speed_scaler = 0.5f + ((float)THROTTLE_CRUISE / channel_throttle->get_servo_out() / 2.0f); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't going to implement that... }else{ speed_scaler = 1.67f; @@ -79,12 +79,12 @@ void Plane::stabilize_roll(float speed_scaler) } bool disable_integrator = false; - if (control_mode == STABILIZE && channel_roll->control_in != 0) { + if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) { disable_integrator = true; } - channel_roll->servo_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, + channel_roll->set_servo_out(rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, - disable_integrator); + disable_integrator)); } /* @@ -98,17 +98,17 @@ void Plane::stabilize_pitch(float speed_scaler) if (force_elevator != 0) { // we are holding the tail down during takeoff. Just convert // from a percentage to a -4500..4500 centidegree angle - channel_pitch->servo_out = 45*force_elevator; + channel_pitch->set_servo_out(45*force_elevator); return; } - int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch; + int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->get_servo_out() * g.kff_throttle_to_pitch; bool disable_integrator = false; - if (control_mode == STABILIZE && channel_pitch->control_in != 0) { + if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) { disable_integrator = true; } - channel_pitch->servo_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, + channel_pitch->set_servo_out (pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, - disable_integrator); + disable_integrator) ); } /* @@ -121,7 +121,7 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) { float ch_inf; - ch_inf = (float)channel->radio_in - (float)channel->radio_trim; + ch_inf = (float)channel->get_radio_in() - (float)channel->get_radio_trim(); ch_inf = fabsf(ch_inf); ch_inf = MIN(ch_inf, 400.0f); ch_inf = ((400.0f - ch_inf) / 400.0f); @@ -129,6 +129,18 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) servo_out += channel->pwm_to_angle(); } +// one argument version when +void Plane::stick_mix_channel(RC_Channel *channel) +{ + float ch_inf; + + ch_inf = (float)channel->get_radio_in() - (float)channel->get_radio_trim(); + ch_inf = fabsf(ch_inf); + ch_inf = MIN(ch_inf, 400.0f); + ch_inf = ((400.0f - ch_inf) / 400.0f); + channel->set_servo_out(channel->get_servo_out() * ch_inf + channel->pwm_to_angle()); +} + /* this gives the user control of the aircraft in stabilization modes */ @@ -148,8 +160,8 @@ void Plane::stabilize_stick_mixing_direct() control_mode == TRAINING) { return; } - stick_mix_channel(channel_roll, channel_roll->servo_out); - stick_mix_channel(channel_pitch, channel_pitch->servo_out); + stick_mix_channel(channel_roll); + stick_mix_channel(channel_pitch); } /* @@ -219,7 +231,7 @@ void Plane::stabilize_yaw(float speed_scaler) } else { // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT - steering_control.ground_steering = (channel_roll->control_in == 0 && + steering_control.ground_steering = (channel_roll->get_control_in() == 0 && fabsf(relative_altitude()) < g.ground_steer_alt); if (control_mode == AUTO && (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || @@ -257,25 +269,25 @@ void Plane::stabilize_yaw(float speed_scaler) void Plane::stabilize_training(float speed_scaler) { if (training_manual_roll) { - channel_roll->servo_out = channel_roll->control_in; + channel_roll->set_servo_out(channel_roll->get_control_in()); } else { // calculate what is needed to hold stabilize_roll(speed_scaler); - if ((nav_roll_cd > 0 && channel_roll->control_in < channel_roll->servo_out) || - (nav_roll_cd < 0 && channel_roll->control_in > channel_roll->servo_out)) { + if ((nav_roll_cd > 0 && channel_roll->get_control_in() < channel_roll->get_servo_out()) || + (nav_roll_cd < 0 && channel_roll->get_control_in() > channel_roll->get_servo_out())) { // allow user to get out of the roll - channel_roll->servo_out = channel_roll->control_in; + channel_roll->set_servo_out(channel_roll->get_control_in()); } } if (training_manual_pitch) { - channel_pitch->servo_out = channel_pitch->control_in; + channel_pitch->set_servo_out(channel_pitch->get_control_in()); } else { stabilize_pitch(speed_scaler); - if ((nav_pitch_cd > 0 && channel_pitch->control_in < channel_pitch->servo_out) || - (nav_pitch_cd < 0 && channel_pitch->control_in > channel_pitch->servo_out)) { + if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < channel_pitch->get_servo_out()) || + (nav_pitch_cd < 0 && channel_pitch->get_control_in() > channel_pitch->get_servo_out())) { // allow user to get back to level - channel_pitch->servo_out = channel_pitch->control_in; + channel_pitch->set_servo_out(channel_pitch->get_control_in()); } } @@ -289,8 +301,8 @@ void Plane::stabilize_training(float speed_scaler) */ void Plane::stabilize_acro(float speed_scaler) { - float roll_rate = (channel_roll->control_in/4500.0f) * g.acro_roll_rate; - float pitch_rate = (channel_pitch->control_in/4500.0f) * g.acro_pitch_rate; + float roll_rate = (channel_roll->get_control_in()/4500.0f) * g.acro_roll_rate; + float pitch_rate = (channel_pitch->get_control_in()/4500.0f) * g.acro_pitch_rate; /* check for special roll handling near the pitch poles @@ -310,16 +322,16 @@ void Plane::stabilize_acro(float speed_scaler) nav_roll_cd = ahrs.roll_sensor + roll_error_cd; // try to reduce the integrated angular error to zero. We set // 'stabilze' to true, which disables the roll integrator - channel_roll->servo_out = rollController.get_servo_out(roll_error_cd, + channel_roll->set_servo_out(rollController.get_servo_out(roll_error_cd, speed_scaler, - true); + true)); } else { /* aileron stick is non-zero, use pure rate control until the user releases the stick */ acro_state.locked_roll = false; - channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler); + channel_roll->set_servo_out(rollController.get_rate_out(roll_rate, speed_scaler)); } if (g.acro_locking && is_zero(pitch_rate)) { @@ -334,15 +346,15 @@ void Plane::stabilize_acro(float speed_scaler) // try to hold the locked pitch. Note that we have the pitch // integrator enabled, which helps with inverted flight nav_pitch_cd = acro_state.locked_pitch_cd; - channel_pitch->servo_out = pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, + channel_pitch->set_servo_out(pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, speed_scaler, - false); + false)); } else { /* user has non-zero pitch input, use a pure rate controller */ acro_state.locked_pitch = false; - channel_pitch->servo_out = pitchController.get_rate_out(pitch_rate, speed_scaler); + channel_pitch->set_servo_out( pitchController.get_rate_out(pitch_rate, speed_scaler)); } /* @@ -387,7 +399,7 @@ void Plane::stabilize() /* see if we should zero the attitude controller integrators. */ - if (channel_throttle->control_in == 0 && + if (channel_throttle->get_control_in() == 0 && relative_altitude_abs_cm() < 500 && fabsf(barometer.get_climb_rate()) < 0.5f && gps.ground_speed() < 3) { @@ -412,11 +424,11 @@ void Plane::calc_throttle() // user has asked for zero throttle - this may be done by a // mission which wants to turn off the engine for a parachute // landing - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); return; } - channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand(); + channel_throttle->set_servo_out(SpdHgt_Controller->get_throttle_demand()); } /***************************************** @@ -435,7 +447,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler) steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator); // add in rudder mixing from roll - steering_control.rudder += channel_roll->servo_out * g.kff_rudder_mix; + steering_control.rudder += channel_roll->get_servo_out() * g.kff_rudder_mix; steering_control.rudder += rudder_input; steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500); } @@ -461,7 +473,7 @@ void Plane::calc_nav_yaw_course(void) void Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && - channel_throttle->control_in == 0 && + channel_throttle->get_control_in() == 0 && flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF && flight_stage != AP_SpdHgtControl::FLIGHT_LAND_ABORT) { // manual rudder control while still @@ -538,12 +550,12 @@ void Plane::throttle_slew_limit(int16_t last_throttle) // if slew limit rate is set to zero then do not slew limit if (slewrate) { // limit throttle change by the given percentage per second - float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min); + float temp = 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)); } } @@ -662,7 +674,7 @@ bool Plane::suppress_throttle(void) /* implement a software VTail or elevon mixer. There are 4 different mixing modes */ -void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out) +void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16_t & chan2_out)const { int16_t c1, c2; int16_t v1, v2; @@ -704,6 +716,17 @@ void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_ chan2_out = 1500 + v2; } +void Plane::channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const +{ + int16_t ch1 = chan1->get_radio_out(); + int16_t ch2 = chan2->get_radio_out(); + + channel_output_mixer(mixing_type,ch1,ch2); + + chan1->set_radio_out(ch1); + chan2->set_radio_out(ch2); +} + /* setup flaperon output channels */ @@ -724,7 +747,7 @@ void Plane::flaperon_update(int8_t flap_percent) by mixing gain). flapin_channel's trim is not used. */ - ch1 = channel_roll->radio_out; + ch1 = channel_roll->get_radio_out(); // The *5 is to take a percentage to a value from -500 to 500 for the mixer ch2 = 1500 - flap_percent * 5; channel_output_mixer(g.flaperon_output, ch1, ch2); @@ -758,9 +781,9 @@ void Plane::set_servos_idle(void) } else { auto_state.idle_wiggle_stage = 0; } - channel_roll->servo_out = servo_value; - channel_pitch->servo_out = servo_value; - channel_rudder->servo_out = servo_value; + channel_roll->set_servo_out(servo_value); + channel_pitch->set_servo_out(servo_value); + channel_rudder->set_servo_out(servo_value); channel_roll->calc_pwm(); channel_pitch->calc_pwm(); channel_rudder->calc_pwm(); @@ -777,9 +800,9 @@ void Plane::set_servos_idle(void) uint16_t Plane::throttle_min(void) const { if (aparm.throttle_min < 0) { - return channel_throttle->radio_trim; + return channel_throttle->get_radio_trim(); } - return channel_throttle->get_reverse() ? channel_throttle->radio_max : channel_throttle->radio_min; + return channel_throttle->get_reverse() ? channel_throttle->get_radio_max() : channel_throttle->get_radio_min(); }; @@ -788,7 +811,7 @@ uint16_t Plane::throttle_min(void) const *****************************************/ void Plane::set_servos(void) { - int16_t last_throttle = channel_throttle->radio_out; + int16_t last_throttle = channel_throttle->get_radio_out(); // do any transition updates for quadplane quadplane.update(); @@ -813,25 +836,25 @@ void Plane::set_servos(void) // steering output steering_control.rudder = steering_control.steering; } - channel_rudder->servo_out = steering_control.rudder; + channel_rudder->set_servo_out(steering_control.rudder); // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run steering_control.ground_steering = false; - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, steering_control.rudder); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, steering_control.steering); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering); if (control_mode == MANUAL) { // do a direct pass through of radio values if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) { - channel_roll->radio_out = channel_roll->radio_in; - channel_pitch->radio_out = channel_pitch->radio_in; + channel_roll->set_radio_out(channel_roll->get_radio_in()); + channel_pitch->set_radio_out(channel_pitch->get_radio_in()); } else { - channel_roll->radio_out = channel_roll->read(); - channel_pitch->radio_out = channel_pitch->read(); + channel_roll->set_radio_out(channel_roll->read()); + channel_pitch->set_radio_out(channel_pitch->read()); } - channel_throttle->radio_out = channel_throttle->radio_in; - channel_rudder->radio_out = channel_rudder->radio_in; + channel_throttle->set_radio_out(channel_throttle->get_radio_in()); + channel_rudder->set_radio_out(channel_rudder->get_radio_in()); // setup extra channels. We want this to come from the // main input channel, but using the 2nd channels dead @@ -839,8 +862,8 @@ void Plane::set_servos(void) // pwm_to_angle_dz() to ensure we don't trim the value for the // deadzone of the main aileron channel, otherwise the 2nd // aileron won't quite follow the first one - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0)); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0)); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0)); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0)); // this variant assumes you have the corresponding // input channel setup in your transmitter for manual control @@ -851,24 +874,24 @@ void Plane::set_servos(void) if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) { // set any differential spoilers to follow the elevons in // manual mode. - RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out); - RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out); + RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->get_radio_out()); + RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->get_radio_out()); } } else { if (g.mix_mode == 0) { // both types of secondary aileron are slaved to the roll servo out - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out()); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out()); // both types of secondary elevator are slaved to the pitch servo out - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out()); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out()); }else{ /*Elevon mode*/ float ch1; float ch2; - ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); - ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out); + ch1 = channel_pitch->get_servo_out() - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out()); + ch2 = channel_pitch->get_servo_out() + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out()); /* Differential Spoilers If differential spoilers are setup, then we translate @@ -879,20 +902,20 @@ void Plane::set_servos(void) if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) { float ch3 = ch1; float ch4 = ch2; - if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) { - ch1 += abs(channel_rudder->servo_out); - ch3 -= abs(channel_rudder->servo_out); + if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->get_servo_out() < 0) { + ch1 += abs(channel_rudder->get_servo_out()); + ch3 -= abs(channel_rudder->get_servo_out()); } else { - ch2 += abs(channel_rudder->servo_out); - ch4 -= abs(channel_rudder->servo_out); + ch2 += abs(channel_rudder->get_servo_out()); + ch4 -= abs(channel_rudder->get_servo_out()); } - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, ch3); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, ch4); } // directly set the radio_out values for elevon mode - channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)); - channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)); + channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX))); + channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX))); } // push out the PWM values @@ -903,7 +926,7 @@ void Plane::set_servos(void) channel_rudder->calc_pwm(); #if THROTTLE_OUT == 0 - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); #else // convert 0 to 100% (or -100 to +100) into PWM int8_t min_throttle = aparm.throttle_min.get(); @@ -933,14 +956,14 @@ void Plane::set_servos(void) // overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value // throttle limit will attack by 10% per second - if (channel_throttle->servo_out > 0 && // demanding too much positive thrust + if (channel_throttle->get_servo_out() > 0 && // demanding too much positive thrust throttle_watt_limit_max < max_throttle - 25 && now - throttle_watt_limit_timer_ms >= 1) { // always allow for 25% throttle available regardless of battery status throttle_watt_limit_timer_ms = now; throttle_watt_limit_max++; - } else if (channel_throttle->servo_out < 0 && + } else if (channel_throttle->get_servo_out() < 0 && min_throttle < 0 && // reverse thrust is available throttle_watt_limit_min < -(min_throttle) - 25 && now - throttle_watt_limit_timer_ms >= 1) { @@ -953,12 +976,12 @@ void Plane::set_servos(void) // it has been 1 second since last over-current, check if we can resume higher throttle. // this throttle release is needed to allow raising the max_throttle as the battery voltage drains down // throttle limit will release by 1% per second - if (channel_throttle->servo_out > throttle_watt_limit_max && // demanding max forward thrust + if (channel_throttle->get_servo_out() > throttle_watt_limit_max && // demanding max forward thrust throttle_watt_limit_max > 0) { // and we're currently limiting it throttle_watt_limit_timer_ms = now; throttle_watt_limit_max--; - } else if (channel_throttle->servo_out < throttle_watt_limit_min && // demanding max negative thrust + } else if (channel_throttle->get_servo_out() < throttle_watt_limit_min && // demanding max negative thrust throttle_watt_limit_min > 0) { // and we're limiting it throttle_watt_limit_timer_ms = now; throttle_watt_limit_min--; @@ -970,19 +993,19 @@ void Plane::set_servos(void) min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0); } - channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, + channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), min_throttle, - max_throttle); + max_throttle)); if (!hal.util->get_soft_armed()) { - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); channel_throttle->calc_pwm(); } else if (suppress_throttle()) { // throttle is suppressed in auto mode - channel_throttle->servo_out = 0; + channel_throttle->set_servo_out(0); if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed - channel_throttle->radio_out = channel_throttle->radio_in; + channel_throttle->set_radio_out(channel_throttle->get_radio_in()); } else { channel_throttle->calc_pwm(); } @@ -994,14 +1017,14 @@ void Plane::set_servos(void) control_mode == AUTOTUNE)) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set - channel_throttle->radio_out = channel_throttle->radio_in; + channel_throttle->set_radio_out(channel_throttle->get_radio_in()); } else if (control_mode == GUIDED && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED - channel_throttle->radio_out = channel_throttle->radio_in; + channel_throttle->set_radio_out(channel_throttle->get_radio_in()); } else if (quadplane.in_vtol_mode()) { // ask quadplane code for forward throttle - channel_throttle->servo_out = quadplane.forward_throttle_pct(); + channel_throttle->set_servo_out(quadplane.forward_throttle_pct()); channel_throttle->calc_pwm(); } else { // normal throttle calculation based on servo_out @@ -1070,8 +1093,8 @@ void Plane::set_servos(void) flap_slew_limit(last_auto_flap, auto_flap_percent); flap_slew_limit(last_manual_flap, manual_flap_percent); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent); if (control_mode >= FLY_BY_WIRE_B || quadplane.in_assisted_flight() || @@ -1083,16 +1106,16 @@ void Plane::set_servos(void) if (control_mode == TRAINING) { // copy rudder in training mode - channel_rudder->radio_out = channel_rudder->radio_in; + channel_rudder->set_radio_out(channel_rudder->get_radio_in()); } if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) { flaperon_update(auto_flap_percent); } if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); + channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder); } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); + channel_output_mixer(g.elevon_output, channel_pitch, channel_roll); } if (!arming.is_armed()) { @@ -1105,12 +1128,12 @@ void Plane::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 = throttle_min(); + channel_throttle->set_radio_out(throttle_min()); break; } } @@ -1143,9 +1166,9 @@ void Plane::set_servos(void) // after an auto land and auto disarm, set the servos to be neutral just // in case we're upside down or some crazy angle and straining the servos. if (g.land_then_servos_neutral == 1) { - channel_roll->radio_out = channel_roll->radio_trim; - channel_pitch->radio_out = channel_pitch->radio_trim; - channel_rudder->radio_out = channel_rudder->radio_trim; + channel_roll->set_radio_out(channel_roll->get_radio_trim()); + channel_pitch->set_radio_out(channel_pitch->get_radio_trim()); + channel_rudder->set_radio_out(channel_rudder->get_radio_trim()); } else if (g.land_then_servos_neutral == 2) { channel_roll->disable_out(); channel_pitch->disable_out(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d2766e0537..988220a93c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -309,7 +309,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) } #endif - if (aparm.throttle_min < 0 && channel_throttle->servo_out < 0) { + if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) { control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR; control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR; } @@ -424,14 +424,14 @@ void Plane::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()); return; } #endif diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index ef6e349828..d4d8e3300f 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -260,8 +260,8 @@ void Plane::Log_Write_Control_Tuning() roll : (int16_t)ahrs.roll_sensor, nav_pitch_cd : (int16_t)nav_pitch_cd, pitch : (int16_t)ahrs.pitch_sensor, - throttle_out : (int16_t)channel_throttle->servo_out, - rudder_out : (int16_t)channel_rudder->servo_out, + throttle_out : (int16_t)channel_throttle->get_servo_out(), + rudder_out : (int16_t)channel_rudder->get_servo_out(), throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -417,7 +417,7 @@ struct PACKED log_Arm_Disarm { void Plane::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/ArduPlane/Plane.h b/ArduPlane/Plane.h index c2efb63cfb..63a76ad8ab 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -994,7 +994,8 @@ private: bool stick_mixing_enabled(void); void stabilize_roll(float speed_scaler); void stabilize_pitch(float speed_scaler); - void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); + static void stick_mix_channel(RC_Channel *channel); + static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); void stabilize_stick_mixing_direct(); void stabilize_stick_mixing_fbw(); void stabilize_yaw(float speed_scaler); @@ -1006,7 +1007,8 @@ private: void throttle_slew_limit(int16_t last_throttle); void flap_slew_limit(int8_t &last_value, int8_t &new_value); bool suppress_throttle(void); - void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out); + void channel_output_mixer(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; + void channel_output_mixer(uint8_t mixing_type, RC_Channel* chan1, RC_Channel* chan2)const; void flaperon_update(int8_t flap_percent); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); diff --git a/ArduPlane/arming_checks.cpp b/ArduPlane/arming_checks.cpp index 94d7e26fb7..a9c4bcdc21 100644 --- a/ArduPlane/arming_checks.cpp +++ b/ArduPlane/arming_checks.cpp @@ -54,7 +54,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool report) if (plane.channel_throttle->get_reverse() && plane.g.throttle_fs_enabled && plane.g.throttle_fs_value < - plane.channel_throttle->radio_max) { + plane.channel_throttle->get_radio_max()) { if (report) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: Invalid THR_FS_VALUE for rev throttle"); } diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 6486912c24..903b2efa06 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -57,12 +57,12 @@ void Plane::failsafe_check(void) // pass RC inputs to outputs every 20ms hal.rcin->clear_overrides(); - channel_roll->radio_out = channel_roll->read(); - channel_pitch->radio_out = channel_pitch->read(); + channel_roll->set_radio_out(channel_roll->read()); + channel_pitch->set_radio_out(channel_pitch->read()); if (hal.util->get_soft_armed()) { - channel_throttle->radio_out = channel_throttle->read(); + channel_throttle->set_radio_out(channel_throttle->read()); } - channel_rudder->radio_out = channel_rudder->read(); + channel_rudder->set_radio_out(channel_rudder->read()); int16_t roll = channel_roll->pwm_to_angle_dz(0); int16_t pitch = channel_pitch->pwm_to_angle_dz(0); @@ -70,15 +70,15 @@ void Plane::failsafe_check(void) // setup secondary output channels that don't have // corresponding input channels - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, roll); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, pitch); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, rudder); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, rudder); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, roll); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, pitch); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, rudder); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, rudder); if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); + channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder); } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); + channel_output_mixer(g.elevon_output, channel_pitch, channel_roll); } #if OBC_FAILSAFE == ENABLED @@ -102,8 +102,8 @@ void Plane::failsafe_check(void) RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input, true); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, 0); - RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, 0); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, 0); + RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, 0); // setup flaperons flaperon_update(0); diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 62f9b4c7e1..8c4d88fa25 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -48,7 +48,7 @@ void QuadPlane::motor_test_output() case MOTOR_TEST_THROTTLE_PERCENT: // sanity check motor_test.throttle value if (motor_test.throttle_value <= 100) { - pwm = plane.channel_throttle->radio_min + (plane.channel_throttle->radio_max - plane.channel_throttle->radio_min) * (float)motor_test.throttle_value/100.0f; + pwm = plane.channel_throttle->get_radio_min() + (plane.channel_throttle->get_radio_max() - plane.channel_throttle->get_radio_min()) * (float)motor_test.throttle_value/100.0f; } break; @@ -57,7 +57,7 @@ void QuadPlane::motor_test_output() break; case MOTOR_TEST_THROTTLE_PILOT: - pwm = plane.channel_throttle->radio_in; + pwm = plane.channel_throttle->get_radio_in(); break; default: diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 9b7e03cab4..e00fad991f 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -94,7 +94,7 @@ void Plane::calc_airspeed_errors() control_mode == CRUISE) { target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) * - channel_throttle->control_in) + + channel_throttle->get_control_in()) + ((int32_t)aparm.airspeed_min * 100); } @@ -203,7 +203,7 @@ void Plane::update_loiter(uint16_t radius) void Plane::update_cruise() { if (!cruise_state.locked_heading && - channel_roll->control_in == 0 && + channel_roll->get_control_in() == 0 && rudder_input == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 3 && @@ -240,7 +240,7 @@ void Plane::update_fbwb_speed_height(void) { static float last_elevator_input; float elevator_input; - elevator_input = channel_pitch->control_in / 4500.0f; + elevator_input = channel_pitch->get_control_in() / 4500.0f; if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp index 051d2bd655..aa90fb0096 100644 --- a/ArduPlane/px4_mixer.cpp +++ b/ArduPlane/px4_mixer.cpp @@ -136,24 +136,24 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) // channels and adjust for trims const RC_Channel *chan1 = RC_Channel::rc_channel(i); const RC_Channel *chan2 = RC_Channel::rc_channel(c1); - int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->radio_trim); - int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->radio_trim); + int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim()); + int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim()); chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); // if the input and output channels are the same then we // apply clipping. This allows for direct pass-thru int32_t limit = (c1==i?scale_max2:scale_max1); int32_t in_scale_low; - if (chan2_trim <= chan2->radio_min) { + if (chan2_trim <= chan2->get_radio_min()) { in_scale_low = scale_max1; } else { - in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->radio_min); + in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->get_radio_min()); } int32_t in_scale_high; - if (chan2->radio_max <= chan2_trim) { + if (chan2->get_radio_max() <= chan2_trim) { in_scale_high = scale_max1; } else { - in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->radio_max - chan2_trim); + in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->get_radio_max() - chan2_trim); } if (chan1->get_reverse() != chan2->get_reverse()) { in_scale_low = -in_scale_low; @@ -161,8 +161,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) } if (!print_buffer(buf, buf_size, "M: 1\n") || !print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", - (int)(pwm_scale*(chan1_trim - chan1->radio_min)), - (int)(pwm_scale*(chan1->radio_max - chan1_trim)), + (int)(pwm_scale*(chan1_trim - chan1->get_radio_min())), + (int)(pwm_scale*(chan1->get_radio_max() - chan1_trim)), (int)(pwm_scale*(chan1_trim - 1500)), (int)-scale_max2, (int)scale_max2) || !print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1, @@ -175,8 +175,8 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) } else { const RC_Channel *chan1 = RC_Channel::rc_channel(c1); const RC_Channel *chan2 = RC_Channel::rc_channel(c2); - int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->radio_trim); - int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->radio_trim); + int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim()); + int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim()); chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); // mix of two input channels to give an output channel. To @@ -315,7 +315,7 @@ bool Plane::setup_failsafe_mixing(void) // by small numbers near RC3_MIN config.rc_trim = 1500; } else { - config.rc_trim = constrain_int16(ch->radio_trim, config.rc_min+1, config.rc_max-1); + config.rc_trim = constrain_int16(ch->get_radio_trim(), config.rc_min+1, config.rc_max-1); } config.rc_dz = 0; // zero for the purposes of manual takeover diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index bdb75da2ee..944e340415 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -460,7 +460,7 @@ void QuadPlane::hold_stabilize(float throttle_in) // quadplane stabilize mode void QuadPlane::control_stabilize(void) { - float pilot_throttle_scaled = plane.channel_throttle->control_in / 100.0f; + float pilot_throttle_scaled = plane.channel_throttle->get_control_in() / 100.0f; hold_stabilize(pilot_throttle_scaled); } @@ -622,8 +622,8 @@ void QuadPlane::control_loiter() pos_control->set_accel_z(pilot_accel_z); // process pilot's roll and pitch input - wp_nav->set_pilot_desired_acceleration(plane.channel_roll->control_in, - plane.channel_pitch->control_in); + wp_nav->set_pilot_desired_acceleration(plane.channel_roll->get_control_in(), + plane.channel_pitch->get_control_in()); // Update EKF speed limit - used to limit speed when we are using optical flow float ekfGndSpdLimit, ekfNavVelGainScaler; @@ -666,7 +666,7 @@ void QuadPlane::control_loiter() */ float QuadPlane::get_pilot_input_yaw_rate_cds(void) { - if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) { + if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) { // the user may be trying to disarm return 0; } @@ -685,7 +685,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void) // use bank angle to get desired yaw rate yaw_cds += desired_auto_yaw_rate_cds(); } - if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) { + if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) { // the user may be trying to disarm return 0; } @@ -706,7 +706,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) return -50; } uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); - uint16_t trim = (plane.channel_throttle->radio_max + plane.channel_throttle->radio_min)/2; + uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min())/2; return pilot_velocity_z_max * plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) / 100.0f; } @@ -716,7 +716,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) */ void QuadPlane::init_throttle_wait(void) { - if (plane.channel_throttle->control_in >= 10 || + if (plane.channel_throttle->get_control_in() >= 10 || plane.is_flying()) { throttle_wait = false; } else { @@ -749,7 +749,7 @@ float QuadPlane::assist_climb_rate_cms(void) } else { // otherwise estimate from pilot input climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); - climb_rate *= plane.channel_throttle->control_in; + climb_rate *= plane.channel_throttle->get_control_in(); } climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up()); return climb_rate; @@ -794,7 +794,7 @@ void QuadPlane::update_transition(void) */ if (have_airspeed && aspeed < assist_speed && (plane.auto_throttle_mode || - plane.channel_throttle->control_in>0 || + plane.channel_throttle->get_control_in()>0 || plane.is_flying())) { // the quad should provide some assistance to the plane transition_state = TRANSITION_AIRSPEED_WAIT; @@ -894,7 +894,7 @@ void QuadPlane::update(void) // disable throttle_wait when throttle rises above 10% if (throttle_wait && - (plane.channel_throttle->control_in > 10 || + (plane.channel_throttle->get_control_in() > 10 || plane.failsafe.ch3_failsafe || plane.failsafe.ch3_counter>0)) { throttle_wait = false; @@ -1620,7 +1620,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) weathervane.last_output = 0; return 0; } - if (plane.channel_rudder->control_in != 0) { + if (plane.channel_rudder->get_control_in() != 0) { weathervane.last_pilot_input_ms = AP_HAL::millis(); weathervane.last_output = 0; return 0; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 0f85dc2ae8..c3f85c2480 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -39,7 +39,7 @@ void Plane::set_control_channels(void) // 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()); } /* @@ -67,7 +67,7 @@ void Plane::init_rc_out() configuration error where the user sets CH3_TRIM incorrectly and the motor may start on power up */ - channel_throttle->radio_trim = throttle_min(); + channel_throttle->set_radio_trim (throttle_min()); if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) { channel_throttle->enable_out(); @@ -102,7 +102,7 @@ void Plane::rudder_arm_disarm_check() } // if throttle is not down, then pilot cannot rudder arm/disarm - if (channel_throttle->control_in != 0){ + if (channel_throttle->get_control_in() != 0){ rudder_arm_timer = 0; return; } @@ -115,7 +115,7 @@ void Plane::rudder_arm_disarm_check() if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter - if (channel_rudder->control_in > 4000) { + if (channel_rudder->get_control_in() > 4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || @@ -135,7 +135,7 @@ void Plane::rudder_arm_disarm_check() } } else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) { // when armed and not flying, full left rudder starts disarming counter - if (channel_rudder->control_in < -4000) { + if (channel_rudder->get_control_in() < -4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || @@ -158,7 +158,7 @@ void Plane::rudder_arm_disarm_check() void Plane::read_radio() { if (!hal.rcin->new_input()) { - control_failsafe(channel_throttle->radio_in); + control_failsafe(channel_throttle->get_radio_in()); return; } @@ -177,8 +177,10 @@ void Plane::read_radio() pwm_roll = elevon.ch1_temp; pwm_pitch = elevon.ch2_temp; }else{ - pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; - pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; + pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; + pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; } RC_Channel::set_pwm_all(); @@ -195,12 +197,12 @@ void Plane::read_radio() channel_pitch->set_pwm(pwm_pitch); } - 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()); - if (g.throttle_nudge && channel_throttle->servo_out > 50 && geofence_stickmixing()) { - float nudge = (channel_throttle->servo_out - 50) * 0.02f; + if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) { + float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; } else { @@ -218,7 +220,7 @@ void Plane::read_radio() // attitude from the roll channel. rudder_input = 0; } else { - rudder_input = channel_rudder->control_in; + rudder_input = channel_rudder->get_control_in(); } // check for transmitter tuning changes @@ -230,17 +232,16 @@ void Plane::control_failsafe(uint16_t pwm) if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) { // we do not have valid RC input. Set all primary channel // control inputs to the trim value and throttle to min - channel_roll->radio_in = channel_roll->radio_trim; - channel_pitch->radio_in = channel_pitch->radio_trim; - channel_rudder->radio_in = channel_rudder->radio_trim; + channel_roll->set_radio_in(channel_roll->get_radio_trim()); + channel_pitch->set_radio_in(channel_pitch->get_radio_trim()); + channel_rudder->set_radio_in(channel_rudder->get_radio_trim()); // note that we don't set channel_throttle->radio_in to radio_trim, // as that would cause throttle failsafe to not activate - - channel_roll->control_in = 0; - channel_pitch->control_in = 0; - channel_rudder->control_in = 0; - channel_throttle->control_in = 0; + channel_roll->set_control_in(0); + channel_pitch->set_control_in(0); + channel_rudder->set_control_in(0); + channel_throttle->set_control_in(0); } if(g.throttle_fs_enabled == 0) @@ -280,12 +281,12 @@ void Plane::control_failsafe(uint16_t pwm) void Plane::trim_control_surfaces() { read_radio(); - int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5; - int16_t trim_pitch_range = (channel_pitch->radio_max - channel_pitch->radio_min)/5; - if (channel_roll->radio_in < channel_roll->radio_min+trim_roll_range || - channel_roll->radio_in > channel_roll->radio_max-trim_roll_range || - channel_pitch->radio_in < channel_pitch->radio_min+trim_pitch_range || - channel_pitch->radio_in > channel_pitch->radio_max-trim_pitch_range) { + int16_t trim_roll_range = (channel_roll->get_radio_max() - channel_roll->get_radio_min())/5; + int16_t trim_pitch_range = (channel_pitch->get_radio_max() - channel_pitch->get_radio_min())/5; + if (channel_roll->get_radio_in() < channel_roll->get_radio_min()+trim_roll_range || + channel_roll->get_radio_in() > channel_roll->get_radio_max()-trim_roll_range || + channel_pitch->get_radio_in() < channel_pitch->get_radio_min()+trim_pitch_range || + channel_pitch->get_radio_in() > channel_pitch->get_radio_max()-trim_pitch_range) { // don't trim for extreme values - if we attempt to trim so // there is less than 20 percent range left then assume the // sticks are not properly centered. This also prevents @@ -296,18 +297,18 @@ void Plane::trim_control_surfaces() // Store control surface trim values // --------------------------------- if(g.mix_mode == 0) { - if (channel_roll->radio_in != 0) { - channel_roll->radio_trim = channel_roll->radio_in; + if (channel_roll->get_radio_in() != 0) { + channel_roll->set_radio_trim(channel_roll->get_radio_in()); } - if (channel_pitch->radio_in != 0) { - channel_pitch->radio_trim = channel_pitch->radio_in; + if (channel_pitch->get_radio_in() != 0) { + channel_pitch->set_radio_trim(channel_pitch->get_radio_in()); } // the secondary aileron/elevator is trimmed only if it has a // corresponding transmitter input channel, which k_aileron // doesn't have - RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron_with_input); - RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_elevator_with_input); + RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_aileron_with_input); + RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::k_elevator_with_input); } else{ if (elevon.ch1_temp != 0) { elevon.trim1 = elevon.ch1_temp; @@ -318,11 +319,11 @@ void Plane::trim_control_surfaces() //Recompute values here using new values for elevon1_trim and elevon2_trim //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed uint16_t center = 1500; - channel_roll->radio_trim = center; - channel_pitch->radio_trim = center; + channel_roll->set_radio_trim(center); + channel_pitch->set_radio_trim(center); } - if (channel_rudder->radio_in != 0) { - channel_rudder->radio_trim = channel_rudder->radio_in; + if (channel_rudder->get_radio_in() != 0) { + channel_rudder->set_radio_trim(channel_rudder->get_radio_in()); } // save to eeprom @@ -354,7 +355,7 @@ bool Plane::rc_failsafe_active(void) return true; } if (channel_throttle->get_reverse()) { - return channel_throttle->radio_in >= g.throttle_fs_value; + return channel_throttle->get_radio_in() >= g.throttle_fs_value; } - return channel_throttle->radio_in <= g.throttle_fs_value; + return channel_throttle->get_radio_in() <= g.throttle_fs_value; } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index d487d0a12d..d269e3067b 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -738,7 +738,7 @@ void Plane::servo_write(uint8_t ch, uint16_t pwm) #if HIL_SUPPORT if (g.hil_mode==1 && !g.hil_servos) { if (ch < 8) { - RC_Channel::rc_channel(ch)->radio_out = pwm; + RC_Channel::rc_channel(ch)->set_radio_out(pwm); } return; } diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 2380dfc5a2..9facd33af1 100644 --- a/ArduPlane/test.cpp +++ b/ArduPlane/test.cpp @@ -60,14 +60,14 @@ int8_t Plane::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", - (int)channel_roll->radio_in, - (int)channel_pitch->radio_in, - (int)channel_throttle->radio_in, - (int)channel_rudder->radio_in, - (int)g.rc_5.radio_in, - (int)g.rc_6.radio_in, - (int)g.rc_7.radio_in, - (int)g.rc_8.radio_in); + (int)channel_roll->get_radio_in(), + (int)channel_pitch->get_radio_in(), + (int)channel_throttle->get_radio_in(), + (int)channel_rudder->get_radio_in(), + (int)g.rc_5.get_radio_in(), + (int)g.rc_6.get_radio_in(), + (int)g.rc_7.get_radio_in(), + (int)g.rc_8.get_radio_in()); if(cliSerial->available() > 0) { return (0); @@ -124,14 +124,14 @@ int8_t Plane::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", - (int)channel_roll->control_in, - (int)channel_pitch->control_in, - (int)channel_throttle->control_in, - (int)channel_rudder->control_in, - (int)g.rc_5.control_in, - (int)g.rc_6.control_in, - (int)g.rc_7.control_in, - (int)g.rc_8.control_in); + (int)channel_roll->get_control_in(), + (int)channel_pitch->get_control_in(), + (int)channel_throttle->get_control_in(), + (int)channel_rudder->get_control_in(), + (int)g.rc_5.get_control_in(), + (int)g.rc_6.get_control_in(), + (int)g.rc_7.get_control_in(), + (int)g.rc_8.get_control_in() ); if(cliSerial->available() > 0) { return (0); @@ -155,7 +155,7 @@ int8_t Plane::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) { hal.scheduler->delay(20); read_radio(); } @@ -164,8 +164,8 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) hal.scheduler->delay(20); read_radio(); - if(channel_throttle->control_in > 0) { - cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->control_in); + if(channel_throttle->get_control_in() > 0) { + cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->get_control_in()); fail_test++; } @@ -177,7 +177,7 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) } if(rc_failsafe_active()) { - cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->radio_in); + cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->get_radio_in()); print_flight_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++;