diff --git a/ArduCopter/AP_Arming_Copter.cpp b/ArduCopter/AP_Arming_Copter.cpp index 72b005b99ba85..6ef88118c4a5a 100644 --- a/ArduCopter/AP_Arming_Copter.cpp +++ b/ArduCopter/AP_Arming_Copter.cpp @@ -631,7 +631,7 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) #if FRAME_CONFIG == HELI_FRAME if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.motors->get_takeoff_collective()) { #else - if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) { + if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->norm_input_dz() > 0) { #endif check_failed(Check::RC, true, "%s too high", rc_item); return false; diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 5931b1559b286..92a1eda85c5d0 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -78,7 +78,7 @@ float Copter::get_pilot_desired_climb_rate_ms() return 0.0f; } - float throttle_control = copter.channel_throttle->get_control_in(); + float throttle_control = copter.channel_throttle->norm_input_dz() * 1000.0f; #if TOY_MODE_ENABLED if (g2.toy_mode.enabled()) { diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 91e6fe6ecaf74..70690823062da 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -873,17 +873,17 @@ void Copter::update_simple_mode(void) if (simple_mode == SimpleMode::SIMPLE) { // rotate roll, pitch input by -initial simple heading (i.e. north facing) - rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw; - pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw; + rollx = channel_roll->norm_input_dz()*simple_cos_yaw - channel_pitch->norm_input_dz()*simple_sin_yaw; + pitchx = channel_roll->norm_input_dz()*simple_sin_yaw + channel_pitch->norm_input_dz()*simple_cos_yaw; }else{ // rotate roll, pitch input by -super simple heading (reverse of heading to home) - rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw; - pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw; + rollx = channel_roll->norm_input_dz()*super_simple_cos_yaw - channel_pitch->norm_input_dz()*super_simple_sin_yaw; + pitchx = channel_roll->norm_input_dz()*super_simple_sin_yaw + channel_pitch->norm_input_dz()*super_simple_cos_yaw; } // rotate roll, pitch input from north facing to vehicle's perspective - channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw()); - channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw()); + channel_roll->set_norm_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw()); + channel_pitch->set_norm_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw()); } // update_super_simple_bearing - adjusts simple bearing based on location diff --git a/ArduCopter/RC_Channel_Copter.cpp b/ArduCopter/RC_Channel_Copter.cpp index 62692858d4fc1..445263856a80d 100644 --- a/ArduCopter/RC_Channel_Copter.cpp +++ b/ArduCopter/RC_Channel_Copter.cpp @@ -221,7 +221,7 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger) case AUX_FUNC::SAVE_TRIM: if ((ch_flag == AuxSwitchPos::HIGH) && (copter.flightmode->allows_save_trim()) && - (copter.channel_throttle->get_control_in() == 0)) { + (is_zero(copter.channel_throttle->norm_input_dz()))) { copter.g2.rc_channels.save_trim(); } break; @@ -237,7 +237,7 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger) } // do not allow saving the first waypoint with zero throttle - if (!copter.mode_auto.mission.present() && (copter.channel_throttle->get_control_in() == 0)) { + if (!copter.mode_auto.mission.present() && (is_zero(copter.channel_throttle->norm_input_dz()))) { break; } @@ -262,7 +262,7 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger) cmd.content.location = copter.current_loc; // if throttle is above zero, create waypoint command - if (copter.channel_throttle->get_control_in() > 0) { + if (copter.channel_throttle->norm_input_dz() > 0.0f) { cmd.id = MAV_CMD_NAV_WAYPOINT; } else { // with zero throttle, create LAND command diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp index 07c3f8d2706a9..4ef7f65ff4222 100644 --- a/ArduCopter/baro_ground_effect.cpp +++ b/ArduCopter/baro_ground_effect.cpp @@ -35,7 +35,7 @@ void Copter::update_ground_effect_detector(void) UNUSED_RESULT(AP::ahrs().get_relative_position_D_origin_float(pos_d_m)); // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag - const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0; + const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->norm_input_dz() > 0.0f; if (!throttle_up && ap.land_complete) { gndeffect_state.takeoff_time_ms = tnow_ms; gndeffect_state.takeoff_alt_m = -pos_d_m; diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 4816774f73d15..8b493454c8a3f 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -59,7 +59,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) // check throttle is at zero read_radio(); - if (channel_throttle->get_control_in() != 0) { + if (!is_zero(channel_throttle->norm_input_dz())) { gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); ap.compass_mot = false; return MAV_RESULT_TEMPORARILY_REJECTED; @@ -148,7 +148,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) // pass through throttle to motors auto &srv = AP::srv(); srv.cork(); - motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); + motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->norm_input_dz()); srv.push(); // read some compass values @@ -158,7 +158,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) battery.read(); // calculate scaling for throttle - throttle_pct = (float)channel_throttle->get_control_in() * 0.001f; + throttle_pct = channel_throttle->norm_input_dz(); throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); // record maximum throttle @@ -220,7 +220,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) if (AP_HAL::millis() - last_send_time_ms > 500) { last_send_time_ms = AP_HAL::millis(); mavlink_msg_compassmot_status_send(gcs_chan.get_chan(), - channel_throttle->get_control_in(), + uint16_t(channel_throttle->norm_input_dz() * 1000.0f), current, interference_pct[0], motor_compensation[0].x, diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 0c1d7ea70e636..8f9db9c89429d 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -35,7 +35,7 @@ void Copter::esc_calibration_startup_check() switch (g.esc_calibrate) { case ESCCalibrationModes::ESCCAL_NONE: // check if throttle is high - if (channel_throttle->get_control_in() >= ESC_CALIBRATION_HIGH_THROTTLE) { + if (channel_throttle->norm_input_dz() >= ESC_CALIBRATION_HIGH_THROTTLE * 0.001f) { // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); // send message to gcs @@ -48,7 +48,7 @@ void Copter::esc_calibration_startup_check() break; case ESCCalibrationModes::ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH: // check if throttle is high - if (channel_throttle->get_control_in() >= ESC_CALIBRATION_HIGH_THROTTLE) { + if (channel_throttle->norm_input_dz() >= ESC_CALIBRATION_HIGH_THROTTLE * 0.001f) { // pass through pilot throttle to escs esc_calibration_passthrough(); } @@ -97,7 +97,7 @@ void Copter::esc_calibration_passthrough() // pass through to motors auto &srv = AP::srv(); srv.cork(); - motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); + motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->norm_input_dz()); srv.push(); } #endif // FRAME_CONFIG != HELI_FRAME diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 234f3f4e1a5b2..cede3fb89687c 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -139,7 +139,7 @@ bool Copter::should_use_landing_swash() const void Copter::heli_update_landing_swash() { motors->set_collective_for_landing(should_use_landing_swash()); - update_collective_low_flag(channel_throttle->get_control_in()); + update_collective_low_flag(int16_t(channel_throttle->norm_input_dz() * 1000.0f)); } // convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1 @@ -148,8 +148,8 @@ float Copter::get_pilot_desired_rotor_speed() const { RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK); if (rc_ptr != nullptr) { - rc_ptr->set_range(1000); - return (float)rc_ptr->get_control_in() * 0.001f; + rc_ptr->set_range(); + return rc_ptr->norm_input_dz(); } return 0.0f; } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 745e927719d46..bb4b1ce35bb68 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -289,7 +289,7 @@ void Copter::update_throttle_mix() if (flightmode->has_manual_throttle()) { // manual throttle - if (channel_throttle->get_control_in() <= 0 && air_mode != AirMode::AIRMODE_ENABLED) { + if (channel_throttle->norm_input_dz() <= 0.0f && air_mode != AirMode::AIRMODE_ENABLED) { attitude_control->set_throttle_mix_min(); } else { attitude_control->set_throttle_mix_man(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index a34a5dc070769..da519eac690fd 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -967,7 +967,8 @@ float Mode::get_pilot_desired_throttle() const mid_stick = 500; } - int16_t throttle_control = channel_throttle->get_control_in(); + // norm_input_dz() on a RANGE channel: 0 at bottom with deadzone, 1 at top; does not use trim + int16_t throttle_control = int16_t(channel_throttle->norm_input_dz() * 1000.0f); // ensure reasonable throttle values throttle_control = constrain_int16(throttle_control,0,1000); diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 28eb084a5026b..dfa596da276e1 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -81,7 +81,7 @@ void ModeAcro_Heli::run() } // get pilot's desired throttle - pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); + pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(int16_t(channel_throttle->norm_input_dz() * 1000.0f)); // output pilot's throttle without angle boost attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index ea52146ecb5b4..a2b7f5b35d452 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -81,9 +81,8 @@ void ModeDrift::run() vel_right_ms = constrain_float(vel_right_ms, -DRIFT_SPEEDLIMIT_MS, DRIFT_SPEEDLIMIT_MS); vel_forward_ms = constrain_float(vel_forward_ms, -DRIFT_SPEEDLIMIT_MS, DRIFT_SPEEDLIMIT_MS); - // roll_input from yaw stick (convert centidegrees -> radians before LP) - // (channel_yaw->get_control_in() returns centidegrees) - const float yaw_stick_rad = cd_to_rad((float)channel_yaw->get_control_in()); + // roll_input from yaw stick (convert to radians before LP) + const float yaw_stick_rad = cd_to_rad(channel_yaw->norm_input_dz() * 4500.0f); roll_input_rad = roll_input_rad * 0.96f + yaw_stick_rad * 0.04f; // convert user input into desired roll velocity term (m/s equivalent) diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index e7337352fea31..c6310f69f214e 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -63,11 +63,11 @@ bool ModeFlip::init(bool ignore_checks) roll_dir = pitch_dir = 0; // choose direction based on pilot's roll and pitch sticks - if (channel_pitch->get_control_in() > 300) { + if (channel_pitch->norm_input_dz() > 300.0f/4500.0f) { pitch_dir = FLIP_PITCH_BACK; - } else if (channel_pitch->get_control_in() < -300) { + } else if (channel_pitch->norm_input_dz() < -300.0f/4500.0f) { pitch_dir = FLIP_PITCH_FORWARD; - } else if (channel_roll->get_control_in() >= 0) { + } else if (channel_roll->norm_input_dz() >= 0.0f) { roll_dir = FLIP_ROLL_RIGHT; } else { roll_dir = FLIP_ROLL_LEFT; @@ -219,7 +219,7 @@ void ModeFlip::abandon_flip() bool ModeFlip::input_is_high_magnitude(RC_Channel &input) const { - return abs(input.get_control_in()) >= 4000; + return fabsf(channel_roll->norm_input_dz()) >= 4000.0f/4500.0f; } #endif diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index e41ee43fa09ee..6ebe25cde6076 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -323,8 +323,8 @@ void ModeFlowHold::run() Vector2f bf_angles_rad; // calculate alt-hold angles - int16_t roll_in = copter.channel_roll->get_control_in(); - int16_t pitch_in = copter.channel_pitch->get_control_in(); + const float roll_in = copter.channel_roll->norm_input_dz(); + const float pitch_in = copter.channel_pitch->norm_input_dz(); const float angle_max_rad = copter.attitude_control->lean_angle_max_rad(); float target_roll_rad, target_pitch_rad; @@ -337,7 +337,7 @@ void ModeFlowHold::run() // don't use for first 3s when we are just taking off Vector2f flow_angles; - flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0)); + flowhold_flow_to_angle(flow_angles, !is_zero(roll_in) || !is_zero(pitch_in)); flow_angles.x = constrain_float(flow_angles.x, -angle_max_rad/2, angle_max_rad/2); flow_angles.y = constrain_float(flow_angles.y, -angle_max_rad/2, angle_max_rad/2); bf_angles_rad += flow_angles; diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 20f06bd2a2684..fb379fc99a008 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -34,7 +34,7 @@ void ModeStabilize_Heli::run() float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads(); // get pilot's desired throttle - pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); + pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(int16_t(channel_throttle->norm_input_dz() * 1000.0f)); // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because // we may be in autorotation flight. This is so that the servos move in a realistic fashion while disarmed diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 9071c930fd314..08750705d538d 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -227,7 +227,7 @@ void ModeSystemId::run() // get pilot's desired throttle #if FRAME_CONFIG == HELI_FRAME - pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); + pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(int16_t(channel_throttle->norm_input_dz() * 1000.0f)); #else pilot_throttle_scaled = get_pilot_desired_throttle(); #endif diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 67295f2acf0a2..aaea6d8262ac9 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -41,7 +41,7 @@ void Copter::auto_disarm_check() thr_low = ap.throttle_zero; } else { float deadband_top = get_throttle_mid() + g.throttle_deadzone; - thr_low = channel_throttle->get_control_in() <= deadband_top; + thr_low = channel_throttle->norm_input_dz() * 1000.0f <= deadband_top; } if (!thr_low || !ap.land_complete) { @@ -137,7 +137,7 @@ void Copter::lost_vehicle_check() } // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch - if (ap.throttle_zero && !motors->armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) { + if (ap.throttle_zero && !motors->armed() && (channel_roll->norm_input_dz() > 4000.0f/4500.0f) && (channel_pitch->norm_input_dz() > 4000.0f/4500.0f)) { if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (AP_Notify::flags.vehicle_lost == false) { AP_Notify::flags.vehicle_lost = true; diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 1b846485889f7..f2aa31844756a 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -26,10 +26,10 @@ void Copter::init_rc_in() channel_yaw = &rc().get_yaw_channel(); // set rc channel ranges - channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); - channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX); - channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX); - channel_throttle->set_range(1000); + channel_roll->set_angle(); + channel_pitch->set_angle(); + channel_yaw->set_angle(); + channel_throttle->set_range(); #if AP_RC_TRANSMITTER_TUNING_ENABLED rc_tuning = rc().find_channel_for_option(RC_Channel::AUX_FUNC::TRANSMITTER_TUNING); @@ -97,13 +97,13 @@ void Copter::read_radio() ap.new_radio_frame = true; set_throttle_and_failsafe(channel_throttle->get_radio_in()); - set_throttle_zero_flag(channel_throttle->get_control_in()); + set_throttle_zero_flag(int16_t(channel_throttle->norm_input_dz() * 1000.0f)); // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters) radio_passthrough_to_motors(); const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f; - rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt); + rc_throttle_control_in_filter.apply(channel_throttle->norm_input_dz() * 1000.0f, dt); last_radio_update_ms = tnow_ms; return; } @@ -201,7 +201,7 @@ void Copter::radio_passthrough_to_motors() { motors->set_radio_passthrough(channel_roll->norm_input(), channel_pitch->norm_input(), - channel_throttle->get_control_in_zero_dz()*0.001f, + channel_throttle->norm_input(), channel_yaw->norm_input()); } @@ -215,5 +215,14 @@ int16_t Copter::get_throttle_mid(void) return g2.toy_mode.get_throttle_mid(); } #endif - return channel_throttle->get_control_mid(); + // Use the geometric mid-point of the PWM range (matches original get_control_mid() behaviour). + // radio_trim is intentionally not used — it affects feel but not stick centre. + const int16_t r_min = channel_throttle->get_radio_min(); + const int16_t r_max = channel_throttle->get_radio_max(); + const int16_t r_mid = (r_min + r_max) / 2; + const int16_t dead_zone_top = r_min + int16_t(channel_throttle->get_dead_zone()); + if (dead_zone_top >= r_max || r_mid <= dead_zone_top) { + return 500; + } + return int16_t(1000.0f * float(r_mid - dead_zone_top) / float(r_max - dead_zone_top)); } diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index 75589c6edc0fc..724631fa5aff1 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -408,11 +408,11 @@ void ToyMode::update() // we use 150 for throttle_at_min to cope with varying stick throws bool throttle_at_min = - copter.channel_throttle->get_control_in() < 150; + copter.channel_throttle->norm_input_dz() < 0.15f; // throttle threshold for throttle arming bool throttle_near_max = - copter.channel_throttle->get_control_in() > 700; + copter.channel_throttle->norm_input_dz() > 0.7f; /* disarm if throttle is low for 1 second when landed @@ -799,9 +799,9 @@ void ToyMode::action_arm(void) // don't arm if sticks aren't in deadzone, to prevent pot problems // on TX causing flight control issues bool sticks_centered = - copter.channel_roll->get_control_in() == 0 && - copter.channel_pitch->get_control_in() == 0 && - copter.channel_yaw->get_control_in() == 0; + is_zero(copter.channel_roll->norm_input_dz()) && + is_zero(copter.channel_pitch->norm_input_dz()) && + is_zero(copter.channel_yaw->norm_input_dz()); if (!sticks_centered) { gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: sticks not centered"); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 658fe46c08555..5b6c744732494 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -179,7 +179,7 @@ float Plane::stabilize_roll_get_roll_out() #endif bool disable_integrator = false; - if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) { + if (control_mode == &mode_stabilize && !is_zero(channel_roll->norm_input_dz())) { disable_integrator = true; } return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator, @@ -249,7 +249,7 @@ float Plane::stabilize_pitch_get_pitch_out() int32_t demanded_pitch = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; bool disable_integrator = false; - if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { + if (control_mode == &mode_stabilize && !is_zero(channel_pitch->norm_input_dz())) { disable_integrator = true; } /* force landing pitch if: @@ -374,7 +374,7 @@ void Plane::stabilize_yaw() } else { // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT - ground_steering = (channel_roll->get_control_in() == 0 && + ground_steering = (is_zero(channel_roll->norm_input_dz()) && fabsf(relative_altitude) < g.ground_steer_alt); if (!landing.is_ground_steering_allowed()) { // don't use ground steering on landing approach @@ -479,7 +479,7 @@ void Plane::stabilize() /* see if we should zero the attitude controller integrators. */ - if (is_zero(get_throttle_input()) && + if (is_zero(get_throttle_input_norm()) && fabsf(relative_altitude) < 5.0f && fabsf(barometer.get_climb_rate()) < 0.5f && ahrs.groundspeed() < 3) { @@ -589,7 +589,7 @@ int16_t Plane::calc_nav_yaw_course(void) int16_t Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && - is_zero(get_throttle_input()) && + is_zero(get_throttle_input_norm()) && flight_stage != AP_FixedWing::FlightStage::TAKEOFF && flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { // manual rudder control while still diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 9f2941ded346e..98a54ad762701 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -699,7 +699,7 @@ void Plane::update_flight_stage(void) if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING); - } else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 && + } else if (landing.get_abort_throttle_enable() && get_throttle_input_norm() >= 0.9f && landing.request_go_around()) { gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 56447fd749992..ff519eab79686 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1215,7 +1215,7 @@ class Plane : public AP_Vehicle { bool have_reverse_throttle_rc_option; bool allow_reverse_thrust(void) const; bool have_reverse_thrust(void) const; - float get_throttle_input(bool no_deadzone=false) const; + float get_throttle_input_norm(bool no_deadzone=false) const; float get_adjusted_throttle_input(bool no_deadzone=false) const; bool reverse_thrust_enabled(UseReverseThrust use_reverse_thrust_option) const; diff --git a/ArduPlane/RC_Channel_Plane.cpp b/ArduPlane/RC_Channel_Plane.cpp index 321a091c7df5f..c64c99eb5eda5 100644 --- a/ArduPlane/RC_Channel_Plane.cpp +++ b/ArduPlane/RC_Channel_Plane.cpp @@ -207,7 +207,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, // setup input throttle as a range. This is needed as init_aux_function is called // after set_control_channels() if (plane.channel_throttle) { - plane.channel_throttle->set_range(100); + plane.channel_throttle->set_range(); } // note that we don't call do_aux_function() here as we don't // want to startup with reverse thrust diff --git a/ArduPlane/VTOL_Assist.cpp b/ArduPlane/VTOL_Assist.cpp index c7711fa375aba..92f2dd0fc2598 100644 --- a/ArduPlane/VTOL_Assist.cpp +++ b/ArduPlane/VTOL_Assist.cpp @@ -65,7 +65,7 @@ bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed) } if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed) - || is_positive(plane.get_throttle_input()) + || is_positive(plane.get_throttle_input_norm()) || plane.is_flying() ) ) { // not in a flight mode and condition where it would be safe to turn on vertical lift motors // skip this check for tailsitters because the forward and vertical motors are the same and are controlled directly by throttle input unlike other quadplanes diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 154a8c2a004d1..83af0a4645c5c 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -65,7 +65,7 @@ void Plane::failsafe_check(void) float roll = roll_in_expo(false); float pitch = pitch_in_expo(false); - float throttle = get_throttle_input(true); + float throttle = get_throttle_input_norm(true) * 100.0f; float rudder = rudder_in_expo(false); if (!arming.is_armed_and_safety_off()) { diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index dfe746fc8bb32..035873495eb6b 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -310,12 +310,12 @@ void Mode::output_pilot_throttle() { if (plane.g.throttle_passthru_stabilize) { // THR_PASS_STAB set, direct mapping - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input_norm(true) * 100.0f); return; } // get throttle, but adjust center to output TRIM_THROTTLE if flight option set - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true) * 100.0f); } // true if throttle min/max limits should be applied diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 9b04667c67fb8..bc624d8a7aadd 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -160,7 +160,7 @@ void ModeAcro::stabilize_quaternion() IGNORE_RETURN(ahrs.get_quaternion(ahrs_q)); // zero target if not flying, no stick input and zero throttle - if (is_zero(plane.get_throttle_input()) && + if (is_zero(plane.get_throttle_input_norm()) && !plane.is_flying() && is_zero(roll_rate) && is_zero(pitch_rate) && diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 33bb57c0160fb..4d2d035dbcd00 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -23,7 +23,7 @@ void ModeCruise::update() roll when heading is locked. Heading becomes unlocked on any aileron or rudder input */ - if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) { + if (!is_zero(plane.channel_roll->norm_input_dz()) || !is_zero(plane.channel_rudder->norm_input_dz())) { locked_heading = false; lock_timer_ms = 0; } @@ -64,7 +64,7 @@ void ModeCruise::navigate() const bool moving_forwards = fabsf(wrap_PI(cd_to_rad(ground_course_cd) - plane.ahrs.get_yaw_rad())) < M_PI_2; if (!locked_heading && - plane.channel_roll->get_control_in() == 0 && + is_zero(plane.channel_roll->norm_input_dz()) && plane.rudder_input() == 0 && plane.gps.status() >= AP_GPS_FixType::FIX_2D && plane.gps.ground_speed() >= GPS_GND_CRS_MIN_SPD && diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index b70b900fb26bd..ed4516fc2217a 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -85,7 +85,7 @@ void ModeGuided::update() // Throttle output if (plane.guided_throttle_passthru) { // manual passthrough of throttle in fence breach - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input_norm(true) * 100.0f); } else if (plane.aparm.throttle_cruise > 1 && plane.guided_state.last_forced_throttle_ms > 0 && diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index db81f64d9d3b6..42325ee03f7c9 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -7,7 +7,7 @@ void ModeManual::update() SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false)); output_rudder_and_steering(plane.rudder_in_expo(false)); - const float throttle = plane.get_throttle_input(true); + const float throttle = plane.get_throttle_input_norm(true) * 100.0f; SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); plane.nav_roll_cd = ahrs.roll_sensor; diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index ad72a21369b26..c3eccb757039a 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -13,12 +13,11 @@ void ModeQStabilize::update() { // set nav_roll and nav_pitch using sticks // Beware that QuadPlane::tailsitter_check_input (called from Plane::read_radio) - // may alter the control_in values for roll and yaw, but not the corresponding - // radio_in values. This means that the results for norm_input would not necessarily - // be correct for tailsitters, so get_control_in() must be used instead. - // normalize control_input to [-1,1] - const float roll_input = (float)plane.channel_roll->get_control_in() / plane.channel_roll->get_range(); - const float pitch_input = (float)plane.channel_pitch->get_control_in() / plane.channel_pitch->get_range(); + // may alter the norm_in values for roll and yaw via set_norm_in(), but not the + // corresponding radio_in values. norm_input_dz() reads back those set_norm_in() + // values, so it is correct for tailsitters here. + const float roll_input = plane.channel_roll->norm_input_dz(); + const float pitch_input = plane.channel_pitch->norm_input_dz(); // then scale to target angles in centidegrees if (plane.quadplane.tailsitter.active()) { diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 883fec1b788fc..f533748a91c47 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -57,7 +57,7 @@ void QuadPlane::motor_test_output() break; case MOTOR_TEST_THROTTLE_PILOT: - pwm = thr_min_pwm + (thr_max_pwm - thr_min_pwm) * plane.get_throttle_input()*0.01f; + pwm = thr_min_pwm + (thr_max_pwm - thr_min_pwm) * plane.get_throttle_input_norm(); break; default: diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 6d8ca8e869a80..01e5896bd792c 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -164,14 +164,14 @@ void Plane::calc_airspeed_errors() } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) { float control_min = 0.0f; float control_mid = 0.0f; - const float control_max = channel_throttle->get_range(); - const float control_in = get_throttle_input(); + const float control_max = 1.0f; + const float control_in = get_throttle_input_norm(); switch (channel_throttle->get_type()) { case RC_Channel::ControlType::ANGLE: control_min = -control_max; break; case RC_Channel::ControlType::RANGE: - control_mid = channel_throttle->get_control_mid(); + control_mid = 0.5f; break; } if (control_in <= control_mid) { @@ -184,8 +184,8 @@ void Plane::calc_airspeed_errors() control_mid, control_max); } } else { - target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) * - get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); + target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) * 100 * + get_throttle_input_norm()) + ((int32_t)aparm.airspeed_min * 100); } #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offboard guided speed change cmd not set, then this section is skipped @@ -410,7 +410,7 @@ void Plane::update_fbwb_speed_height(void) target_altitude.last_elev_check_us = now; - float elevator_input = channel_pitch->get_control_in() * (1/4500.0); + float elevator_input = channel_pitch->norm_input_dz(); if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 554067237445d..964b7640766ac 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -28,7 +28,7 @@ float QAutoTune::get_desired_climb_rate_ms(void) const void QAutoTune::get_pilot_desired_rp_yrate_rad(float &des_roll_rad, float &des_pitch_rad, float &des_yaw_rate_rads) { - if (plane.channel_roll->get_control_in() == 0 && plane.channel_pitch->get_control_in() == 0) { + if (is_zero(plane.channel_roll->norm_input_dz()) && is_zero(plane.channel_pitch->norm_input_dz())) { des_roll_rad = 0.0; des_pitch_rad = 0.0; } else { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 57f3c89e55237..910391523f052 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -899,7 +899,7 @@ void QuadPlane::run_esc_calibration(void) switch (esc_calibration) { case 1: // throttle based calibration - motors->set_throttle_passthrough_for_esc_calibration(plane.get_throttle_input() * 0.01f); + motors->set_throttle_passthrough_for_esc_calibration(plane.get_throttle_input_norm()); break; case 2: // full range calibration @@ -1118,11 +1118,8 @@ void QuadPlane::hold_hover(float target_climb_rate_cms) float QuadPlane::get_pilot_throttle() { - // get scaled throttle input - float throttle_in = plane.channel_throttle->get_control_in(); - - // normalize to [0,1] - throttle_in /= plane.channel_throttle->get_range(); + // get normalized throttle input [0,1] + float throttle_in = plane.channel_throttle->norm_input_dz(); if (is_positive(throttle_expo)) { // get hover throttle level [0,1] @@ -1150,8 +1147,8 @@ void QuadPlane::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_o } // fetch roll and pitch inputs - roll_out_cd = plane.channel_roll->get_control_in(); - pitch_out_cd = plane.channel_pitch->get_control_in(); + roll_out_cd = plane.channel_roll->norm_input_dz() * 4500.0f; + pitch_out_cd = plane.channel_pitch->norm_input_dz() * 4500.0f; // limit max lean angle, always allow for 10 degrees angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd); @@ -1182,11 +1179,8 @@ float QuadPlane::get_pilot_land_throttle(void) const // assume zero throttle if lost RC return 0; } - // get scaled throttle input - float throttle_in = plane.channel_throttle->get_control_in(); - - // normalize to [0,1] - throttle_in /= plane.channel_throttle->get_range(); + // get normalized throttle input [0,1] + float throttle_in = plane.channel_throttle->norm_input_dz(); return constrain_float(throttle_in, 0, 1); } @@ -1253,7 +1247,7 @@ bool QuadPlane::is_flying_vtol(void) const } if (plane.control_mode->is_vtol_man_mode()) { // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero - return is_positive(get_throttle_input()); + return is_positive(get_throttle_input_norm()); } if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) { // use landing detector @@ -1320,10 +1314,10 @@ float QuadPlane::landing_descent_rate_ms(float height_above_ground_m) */ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { - const auto rudder_in = plane.channel_rudder->get_control_in(); + const float rudder_in = plane.channel_rudder->norm_input_dz(); bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); if (!manual_air_mode && - !is_positive(get_throttle_input()) && + !is_positive(get_throttle_input_norm()) && (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) && plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM && rudder_in < 0 && @@ -1355,7 +1349,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } - return input_expo(rudder_in * (1/4500.0), command_model_pilot.get_expo()) * max_rate * 100.0; + return input_expo(rudder_in, command_model_pilot.get_expo()) * max_rate * 100.0; } /* @@ -1390,7 +1384,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const } uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min()) / 2; - const float throttle_request = plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) * 0.01f; + const float throttle_request = plane.channel_throttle->norm_input_dz_trim(dead_zone, trim); return throttle_request * (throttle_request > 0.0f ? pilot_speed_z_max_up_ms : get_pilot_velocity_z_max_dn_m()) * 100; } @@ -1400,7 +1394,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const */ void QuadPlane::init_throttle_wait(void) { - if (get_throttle_input() >= 10 || + if (get_throttle_input_norm() >= 0.1f || plane.is_flying()) { throttle_wait = false; } else { @@ -1440,7 +1434,7 @@ float QuadPlane::assist_climb_rate_cms(void) const } else { // otherwise estimate from pilot input climb_rate_cms = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd / (plane.aparm.pitch_limit_max * 100)); - climb_rate_cms *= plane.get_throttle_input(); + climb_rate_cms *= plane.get_throttle_input_norm() * 100.0f; } climb_rate_cms = constrain_float(climb_rate_cms, -wp_nav->get_default_speed_down_ms() * 100.0, wp_nav->get_default_speed_up_ms() * 100.0); @@ -1791,7 +1785,7 @@ void QuadPlane::update(void) // disable throttle_wait when throttle rises above 10% if (throttle_wait && - (plane.get_throttle_input() > 10 || + (plane.get_throttle_input_norm() > 0.1f || !rc().has_valid_input())) { throttle_wait = false; } @@ -1873,7 +1867,7 @@ void QuadPlane::update_throttle_suppression(void) consider non-zero throttle to mean that pilot is commanding takeoff unless in a manual throttle mode */ - if (!is_zero(get_throttle_input()) && + if (!is_zero(get_throttle_input_norm()) && (rc().arming_check_throttle() || plane.control_mode->is_vtol_man_throttle() || plane.channel_throttle->norm_input_dz() > 0)) { @@ -2184,9 +2178,8 @@ void QuadPlane::update_land_positioning(void) poscontrol.target_vel_ms.zero(); return; } - const float scale = 1.0 / 4500; - float roll_in = plane.channel_roll->get_control_in() * scale; - float pitch_in = plane.channel_pitch->get_control_in() * scale; + float roll_in = plane.channel_roll->norm_input_dz(); + float pitch_in = plane.channel_pitch->norm_input_dz(); // limit correction speed to accel with stopping time constant of 0.5s const float speed_max_ms = wp_nav->get_wp_acceleration_mss() * 0.5; @@ -3930,7 +3923,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) const bool is_takeoff = in_vtol_auto() && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id); float wv_output; if (weathervane->get_yaw_out(wv_output, - plane.channel_rudder->get_control_in(), + int16_t(plane.channel_rudder->norm_input_dz() * 4500.0f), plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING), pos_control->get_roll_cd(), pos_control->get_pitch_cd(), @@ -4167,7 +4160,7 @@ void QuadPlane::update_throttle_mix(void) if (plane.control_mode->is_vtol_man_throttle()) { // manual throttle - if (!is_positive(get_throttle_input()) && !air_mode_active()) { + if (!is_positive(get_throttle_input_norm()) && !air_mode_active()) { attitude_control->set_throttle_mix_min(); } else { attitude_control->set_throttle_mix_man(); @@ -4887,12 +4880,12 @@ bool QuadPlane::should_disable_TECS() const } // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe! -// This is a re-implmentation of Plane::get_throttle_input +// This is a re-implmentation of Plane::get_throttle_input_norm // Ignoring the no_deadzone case means we don't need to check for valid RC // This is handled by Plane::control_failsafe setting of control in -float QuadPlane::get_throttle_input() const +float QuadPlane::get_throttle_input_norm() const { - float ret = plane.channel_throttle->get_control_in(); + float ret = plane.channel_throttle->norm_input_dz(); if (plane.reversed_throttle) { // RC option for reverse throttle has been set ret = -ret; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index eb0601874b565..06ac0ccd73d01 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -191,7 +191,7 @@ class QuadPlane bool should_disable_TECS() const; // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe! - float get_throttle_input() const; + float get_throttle_input_norm() const; void Log_Write_AttRate(); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 20ac87db192d4..6c14e4e035e31 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -21,20 +21,20 @@ void Plane::set_control_channels(void) channel_rudder = &rc().get_yaw_channel(); // set rc channel ranges - channel_roll->set_angle(SERVO_MAX); - channel_pitch->set_angle(SERVO_MAX); - channel_rudder->set_angle(SERVO_MAX); + channel_roll->set_angle(); + channel_pitch->set_angle(); + channel_rudder->set_angle(); if (!have_reverse_thrust()) { // normal operation - channel_throttle->set_range(100); + channel_throttle->set_range(); } else { // reverse thrust if (have_reverse_throttle_rc_option) { // when we have a reverse throttle RC option setup we use throttle // as a range, and rely on the RC switch to get reverse thrust - channel_throttle->set_range(100); + channel_throttle->set_range(); } else { - channel_throttle->set_angle(100); + channel_throttle->set_angle(); } SRV_Channels::set_angle(SRV_Channel::k_throttle, 100); SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 100); @@ -142,9 +142,9 @@ void Plane::read_radio() airspeed_nudge_cm = 0; throttle_nudge = 0; if (g.throttle_nudge - && channel_throttle->get_control_in() > 50 + && channel_throttle->norm_input_dz() > 0.5f && stickmixing) { - float nudge = (channel_throttle->get_control_in() - 50) * 0.02f; + float nudge = (channel_throttle->norm_input_dz() - 0.5f) * 2.0f; if (TECS_controller.use_airspeed()) { airspeed_nudge_cm = (aparm.airspeed_max - aparm.airspeed_cruise) * nudge * 100; } else { @@ -178,7 +178,7 @@ int16_t Plane::rudder_input(void) } if (stick_mixing_enabled()) { - return channel_rudder->get_control_in(); + return int16_t(channel_rudder->norm_input_dz() * 4500.0f); } return 0; @@ -196,9 +196,9 @@ void Plane::control_failsafe() // note that we don't set channel_throttle->radio_in to radio_trim, // as that would cause throttle failsafe to not activate - channel_roll->set_control_in(0); - channel_pitch->set_control_in(0); - channel_rudder->set_control_in(0); + channel_roll->set_norm_in(0.0f); + channel_pitch->set_norm_in(0.0f); + channel_rudder->set_norm_in(0.0f); airspeed_nudge_cm = 0; throttle_nudge = 0; @@ -216,13 +216,13 @@ void Plane::control_failsafe() #endif if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE) { // set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone - channel_throttle->set_control_in(channel_throttle->get_range() / 2); + channel_throttle->set_norm_in(0.5f); break; } FALLTHROUGH; #endif default: - channel_throttle->set_control_in(0); + channel_throttle->set_norm_in(0.0f); break; } } @@ -275,8 +275,8 @@ void Plane::trim_radio() return; } - if (labs(channel_roll->get_control_in()) > (channel_roll->get_range() * 0.2) || - labs(channel_pitch->get_control_in()) > (channel_pitch->get_range() * 0.2)) { + if (fabsf(channel_roll->norm_input_dz()) > 0.2f || + fabsf(channel_pitch->norm_input_dz()) > 0.2f) { // don't trim for extreme values - if we attempt to trim // more than 20 percent range left then assume the // sticks are not properly centered. This also prevents @@ -365,8 +365,8 @@ static float channel_expo(RC_Channel *chan, int8_t expo, bool use_dz) if (chan == nullptr) { return 0; } - float rin = use_dz? chan->get_control_in() : chan->get_control_in_zero_dz(); - return SERVO_MAX * expo_curve(constrain_float(expo*0.01, 0, 1), rin/SERVO_MAX); + float rin = use_dz? chan->norm_input_dz() : chan->norm_input(); + return SERVO_MAX * expo_curve(constrain_float(expo*0.01, 0, 1), rin); } float Plane::roll_in_expo(bool use_dz) const diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index ed560090c12ab..7a3d71c2cdea6 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -127,9 +127,9 @@ bool Plane::have_reverse_thrust(void) const } /* - return control in from the radio throttle channel. + return control in from the radio throttle channel. (0 to 1 or -1 to 1) */ -float Plane::get_throttle_input(bool no_deadzone) const +float Plane::get_throttle_input_norm(bool no_deadzone) const { if (!rc().has_valid_input()) { // Return 0 if there is no valid input @@ -137,9 +137,9 @@ float Plane::get_throttle_input(bool no_deadzone) const } float ret; if (no_deadzone) { - ret = channel_throttle->get_control_in_zero_dz(); + ret = channel_throttle->norm_input(); } else { - ret = channel_throttle->get_control_in(); + ret = channel_throttle->norm_input_dz(); } if (reversed_throttle) { // RC option for reverse throttle has been set @@ -159,9 +159,9 @@ float Plane::get_adjusted_throttle_input(bool no_deadzone) const } if ((plane.channel_throttle->get_type() != RC_Channel::ControlType::RANGE) || (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) { - return get_throttle_input(no_deadzone); + return get_throttle_input_norm(no_deadzone); } - float ret = channel_throttle->get_range() * throttle_curve(aparm.throttle_cruise * 0.01, 0, 0.5 + 0.5*channel_throttle->norm_input()); + float ret = throttle_curve(aparm.throttle_cruise * 0.01, 0, 0.5 + 0.5*channel_throttle->norm_input()); if (reversed_throttle) { // RC option for reverse throttle has been set return -ret; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 5fb8524b7ae68..e0b23b50f29e7 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -630,7 +630,7 @@ void Plane::set_throttle(void) if (suppress_throttle()) { if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input_norm(true) * 100.0f); } else if (landing.is_flaring() && landing.use_thr_min_during_flare() ) { // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 55030d5ea2ce5..3e71347a0b799 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -596,10 +596,10 @@ void Tailsitter::check_input(void) // is active. We switch around the control_in value for the // channels to do this, as that ensures the value is // consistent throughout the code - int16_t roll_in = plane.channel_roll->get_control_in(); - int16_t yaw_in = plane.channel_rudder->get_control_in(); - plane.channel_roll->set_control_in(yaw_in); - plane.channel_rudder->set_control_in(-roll_in); + const float roll_in = plane.channel_roll->norm_input_dz(); + const float yaw_in = plane.channel_rudder->norm_input_dz(); + plane.channel_roll->set_norm_in(yaw_in); + plane.channel_rudder->set_norm_in(-roll_in); } } diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 3dae45be4e066..f061f295af443 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -566,8 +566,7 @@ void Tiltrotor::vectoring(void) // this test is subject to wrapping at ~49 days, but the consequences are insignificant if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) { if (quadplane.in_vtol_mode()) { - float yaw_out = plane.channel_rudder->get_control_in(); - yaw_out /= plane.channel_rudder->get_range(); + float yaw_out = plane.channel_rudder->norm_input_dz(); float yaw_range = zero_out; SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1)); diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index f6b1464f9000b..5fc888f785057 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -96,14 +96,14 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) } // ensure a reasonable throttle value - throttle_control = constrain_float(throttle_control,0.0f,1000.0f); + throttle_control = constrain_float(throttle_control, 0.0f, 1.0f); // ensure a reasonable deadzone g.throttle_deadzone.set(constrain_int16(g.throttle_deadzone, 0, 400)); - float mid_stick = channel_throttle->get_control_mid(); - float deadband_top = mid_stick + g.throttle_deadzone * gain; - float deadband_bottom = mid_stick - g.throttle_deadzone * gain; + float mid_stick = 0.5f; + float deadband_top = mid_stick + g.throttle_deadzone * gain / 1000.0f; + float deadband_bottom = mid_stick - g.throttle_deadzone * gain / 1000.0f; // check throttle is above, below or in the deadband if (throttle_control < deadband_bottom) { @@ -111,7 +111,7 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) return get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom; } else if (throttle_control > deadband_top) { // above the deadband - return g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top); + return g.pilot_speed_up * (throttle_control-deadband_top) / (1.0f-deadband_top); } else { // must be in the deadband return 0.0f; diff --git a/ArduSub/mode_acro.cpp b/ArduSub/mode_acro.cpp index 55da2e214e384..7bf88efcb4035 100644 --- a/ArduSub/mode_acro.cpp +++ b/ArduSub/mode_acro.cpp @@ -27,13 +27,13 @@ void ModeAcro::run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // convert the input to the desired body frame rate - get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); + get_pilot_desired_angle_rates(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, channel_yaw->norm_input_dz() * 4500.0f, target_roll, target_pitch, target_yaw); // run attitude controller attitude_control->input_rate_bf_roll_pitch_yaw_cds(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost - attitude_control->set_throttle_out((channel_throttle->norm_input() + 1.0f) / 2.0f, false, g.throttle_filt); + attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); //control_in is range 0-1000 //radio_in is raw pwm value diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index eeed9b7c80269..04949b7ba3276 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -70,10 +70,10 @@ void ModeAlthold::run_pre() return; } - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->get_althold_lean_angle_max_cd()); // get pilot's desired yaw rate - float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()); + float yaw_input = channel_yaw->norm_input_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()) * 4500.0f; float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input); // call attitude controller @@ -112,7 +112,7 @@ void ModeAlthold::control_depth() { distance_to_surface = constrain_float(distance_to_surface, 0.0f, 1.0f); motors.set_max_throttle(g.surface_max_throttle + (1.0f - g.surface_max_throttle) * distance_to_surface); - float target_climb_rate_cms = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + float target_climb_rate_cms = sub.get_pilot_desired_climb_rate(channel_throttle->norm_input_dz()); target_climb_rate_cms = constrain_float(target_climb_rate_cms, -sub.get_pilot_speed_dn(), g.pilot_speed_up); // desired_climb_rate returns 0 when within the deadzone. diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index 9564cab598931..3d7d97e43269e 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -121,7 +121,7 @@ void ModeAuto::auto_wp_run() float target_yaw_rate = 0; if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -155,7 +155,7 @@ void ModeAuto::auto_wp_run() // get pilot desired lean angles float target_roll, target_pitch; - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->lean_angle_max_cd()); // call attitude controller if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { @@ -249,7 +249,7 @@ void ModeAuto::auto_circle_run() position_control->D_update_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw_cd(), true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, sub.circle_nav.get_yaw_cd(), true); } #if NAV_GUIDED @@ -312,7 +312,7 @@ void ModeAuto::auto_loiter_run() // accept pilot input of yaw float target_yaw_rate = 0; if (!sub.failsafe.pilot_input) { - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); } // set motors to full range @@ -336,7 +336,7 @@ void ModeAuto::auto_loiter_run() // get pilot desired lean angles float target_roll, target_pitch; - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->lean_angle_max_cd()); // roll & pitch & yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); @@ -573,7 +573,7 @@ void ModeAuto::auto_terrain_recover_run() // convert pilot input to lean angles // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->lean_angle_max_cd()); float target_yaw_rate = 0; diff --git a/ArduSub/mode_circle.cpp b/ArduSub/mode_circle.cpp index c3fa43b267989..f3cb83ab81dcd 100644 --- a/ArduSub/mode_circle.cpp +++ b/ArduSub/mode_circle.cpp @@ -54,13 +54,13 @@ void ModeCircle::run() // process pilot inputs // get pilot's desired yaw rate - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); if (!is_zero(target_yaw_rate)) { sub.circle_pilot_yaw_override = true; } // get pilot desired climb rate - target_climb_rate = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate = sub.get_pilot_desired_climb_rate(channel_throttle->norm_input_dz()); // set motors to full range motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); @@ -80,9 +80,9 @@ void ModeCircle::run() // call attitude controller if (sub.circle_pilot_yaw_override) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else { - attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw_cd(), true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, sub.circle_nav.get_yaw_cd(), true); } // update altitude target and call position controller diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index c7880e2fc07ab..80db6208c14f3 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -474,7 +474,7 @@ void ModeGuided::guided_pos_control_run() float target_yaw_rate = 0; if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } else{ @@ -506,18 +506,18 @@ void ModeGuided::guided_pos_control_run() // call attitude controller if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) { // roll, pitch from pilot, yaw & yaw_rate from auto_control target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; - attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, get_auto_heading(), target_yaw_rate); } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) { // roll, pitch from pilot, yaw_rate from auto_control target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, get_auto_heading(), true); } } @@ -541,7 +541,7 @@ void ModeGuided::guided_vel_control_run() float target_yaw_rate = 0; if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } else{ @@ -579,18 +579,18 @@ void ModeGuided::guided_vel_control_run() // call attitude controller if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) { // roll, pitch from pilot, yaw & yaw_rate from auto_control target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; - attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, get_auto_heading(), target_yaw_rate); } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) { // roll, pitch from pilot, yaw_rate from auto_control target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, get_auto_heading(), true); } } @@ -615,7 +615,7 @@ void ModeGuided::guided_posvel_control_run() if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } else{ @@ -659,18 +659,18 @@ void ModeGuided::guided_posvel_control_run() // call attitude controller if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) { // roll, pitch from pilot, yaw & yaw_rate from auto_control target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; - attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, get_auto_heading(), target_yaw_rate); } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) { // roll, pitch from pilot, and yaw_rate from auto_control target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, get_auto_heading(), true); } } diff --git a/ArduSub/mode_manual.cpp b/ArduSub/mode_manual.cpp index ae4b3db65ea2c..70009792c9866 100644 --- a/ArduSub/mode_manual.cpp +++ b/ArduSub/mode_manual.cpp @@ -29,7 +29,7 @@ void ModeManual::run() sub.motors.set_roll(channel_roll->norm_input()); sub.motors.set_pitch(channel_pitch->norm_input()); sub.motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); - sub.motors.set_throttle((channel_throttle->norm_input() + 1.0f) / 2.0f); + sub.motors.set_throttle(channel_throttle->norm_input()); sub.motors.set_forward(channel_forward->norm_input()); sub.motors.set_lateral(channel_lateral->norm_input()); } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index 0cf8d766a1753..f31096142b7f2 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -59,13 +59,13 @@ void ModePoshold::run() // Update attitude // // get pilot's desired yaw rate - float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()); + float yaw_input = channel_yaw->norm_input_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()) * 4500.0f; float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats float target_roll, target_pitch; - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->lean_angle_max_cd()); // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp index 579412952e992..9f435641da358 100644 --- a/ArduSub/mode_stabilize.cpp +++ b/ArduSub/mode_stabilize.cpp @@ -29,10 +29,10 @@ void ModeStabilize::run() // convert pilot input to lean angles // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats // TODO2: move into mode.h - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->lean_angle_max_cd()); // get pilot's desired yaw rate - float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()); + float yaw_input = channel_yaw->norm_input_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim()) * 4500.0f; float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input); // call attitude controller @@ -60,7 +60,7 @@ void ModeStabilize::run() } // output pilot's throttle - attitude_control->set_throttle_out((channel_throttle->norm_input() + 1.0f) / 2.0f, false, g.throttle_filt); + attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); //control_in is range -1000-1000 //radio_in is raw pwm value diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp index cea46e8240547..76a73414deb40 100644 --- a/ArduSub/mode_surface.cpp +++ b/ArduSub/mode_surface.cpp @@ -43,10 +43,10 @@ void ModeSurface::run() // convert pilot input to lean angles // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->norm_input_dz() * 4500.0f, channel_pitch->norm_input_dz() * 4500.0f, target_roll, target_pitch, attitude_control->lean_angle_max_cd()); // get pilot's desired yaw rate - float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz() * 4500.0f); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp index 560c79951e8fb..8458cff56996c 100644 --- a/ArduSub/mode_surftrak.cpp +++ b/ArduSub/mode_surftrak.cpp @@ -110,7 +110,7 @@ void ModeSurftrak::reset() * Main controller, call at 100hz+ */ void ModeSurftrak::control_range() { - float target_climb_rate_cms = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + float target_climb_rate_cms = sub.get_pilot_desired_climb_rate(channel_throttle->norm_input_dz()); target_climb_rate_cms = constrain_float(target_climb_rate_cms, -sub.get_pilot_speed_dn(), g.pilot_speed_up); // Desired_climb_rate returns 0 when within the deadzone diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index 546cb514af195..2d52b83f16b85 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -10,12 +10,12 @@ void Sub::init_rc_in() channel_lateral = &rc().get_lateral_channel(); // set rc channel ranges - channel_roll->set_angle(ROLL_PITCH_INPUT_MAX); - channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX); - channel_yaw->set_angle(ROLL_PITCH_INPUT_MAX); - channel_throttle->set_range(1000); - channel_forward->set_angle(ROLL_PITCH_INPUT_MAX); - channel_lateral->set_angle(ROLL_PITCH_INPUT_MAX); + channel_roll->set_angle(); + channel_pitch->set_angle(); + channel_yaw->set_angle(); + channel_throttle->set_range(); + channel_forward->set_angle(); + channel_lateral->set_angle(); // set default dead zones channel_roll->set_default_dead_zone(30); diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 3173343f27e45..2dbc6384b9a21 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -379,7 +379,7 @@ class Blimp : public AP_Vehicle void enable_motor_output(); void read_radio(); void set_throttle_and_failsafe(uint16_t throttle_pwm); - void set_throttle_zero_flag(int16_t throttle_control); + void set_throttle_zero_flag(float throttle_control); // sensors.cpp void read_barometer(void); diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index fb42a44111fe0..a44006dc56cb4 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -168,10 +168,10 @@ void Mode::get_pilot_input(Vector3f &pilot, float &yaw) return; } // fetch pilot inputs - pilot.y = channel_right->get_control_in() / float(INPUT_AND_OUTPUT_SCALING); - pilot.x = channel_front->get_control_in() / float(INPUT_AND_OUTPUT_SCALING); - pilot.z = -channel_up->get_control_in() / float(INPUT_AND_OUTPUT_SCALING); - yaw = channel_yaw->get_control_in() / float(INPUT_AND_OUTPUT_SCALING); + pilot.y = channel_right->norm_input_dz(); + pilot.x = channel_front->norm_input_dz(); + pilot.z = -channel_up->norm_input_dz; + yaw = channel_yaw->norm_input_dz(); } bool Mode::is_disarmed_or_landed() const diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index a0783daf9c4a4..ef8907e67e86b 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -21,10 +21,11 @@ void Blimp::init_rc_in() channel_yaw = &rc().get_yaw_channel(); // set rc channel ranges - channel_right->set_angle(INPUT_AND_OUTPUT_SCALING); - channel_front->set_angle(INPUT_AND_OUTPUT_SCALING); - channel_yaw->set_angle(INPUT_AND_OUTPUT_SCALING); - channel_up->set_angle(INPUT_AND_OUTPUT_SCALING); + channel_right->set_angle(); + channel_front->set_angle(); + channel_yaw->set_angle(); + channel_up->set_angle(); + channel_up->set_angle(); // set default dead zones default_dead_zones(); @@ -59,10 +60,10 @@ void Blimp::read_radio() ap.new_radio_frame = true; set_throttle_and_failsafe(channel_up->get_radio_in()); - set_throttle_zero_flag(channel_up->get_control_in()); + set_throttle_zero_flag(channel_up->norm_input_dz()); const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f; - rc_throttle_control_in_filter.apply(channel_up->get_control_in(), dt); + rc_throttle_control_in_filter.apply(channel_up->norm_input_dz(), dt); last_radio_update_ms = tnow_ms; return; } @@ -138,7 +139,7 @@ void Blimp::set_throttle_and_failsafe(uint16_t throttle_pwm) // throttle_zero is used to determine if the pilot intends to shut down the motors // Basically, this signals when we are not flying. We are either on the ground // or the pilot has shut down the vehicle in the air and it is free-floating -void Blimp::set_throttle_zero_flag(int16_t throttle_control) +void Blimp::set_throttle_zero_flag(float throttle_control) { static uint32_t last_nonzero_throttle_ms = 0; uint32_t tnow_ms = millis(); diff --git a/Rover/Log.cpp b/Rover/Log.cpp index acb555e9c2229..763a368f2d4a4 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -196,7 +196,7 @@ void Rover::Log_Write_Steering() struct log_Steering pkt = { LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG), time_us : AP_HAL::micros64(), - steering_in : channel_steer->get_control_in(), + steering_in : (int16_t)(channel_steer->norm_input_dz() * 4500.0f), steering_out : g2.motors.get_steering(), desired_lat_accel : control_mode->get_desired_lat_accel(), lat_accel : lat_accel, @@ -225,7 +225,7 @@ void Rover::Log_Write_Throttle() struct log_Throttle pkt = { LOG_PACKET_HEADER_INIT(LOG_THR_MSG), time_us : AP_HAL::micros64(), - throttle_in : channel_throttle->get_control_in(), + throttle_in : (int16_t)(channel_throttle->norm_input_dz() * 1000.0f), throttle_out : g2.motors.get_throttle(), desired_speed : g2.attitude_control.get_desired_speed(), speed : speed, diff --git a/Rover/mode.cpp b/Rover/mode.cpp index f03422a251cb1..436c9c098575c 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -73,9 +73,9 @@ void Mode::get_pilot_input(float &steering_out, float &throttle_out) const case PilotSteerType::DIR_REVERSED_WHEN_REVERSING: default: { // by default regular and skid-steering vehicles reverse their rotation direction when backing up - throttle_out = rover.channel_throttle->get_control_in(); + throttle_out = rover.channel_throttle->norm_input_dz() * 100.0f; const float steering_dir = is_negative(throttle_out) ? -1 : 1; - steering_out = steering_dir * rover.channel_steer->get_control_in(); + steering_out = steering_dir * rover.channel_steer->norm_input_dz() * 4500.0f; break; } @@ -93,8 +93,8 @@ void Mode::get_pilot_input(float &steering_out, float &throttle_out) const } case PilotSteerType::DIR_UNCHANGED_WHEN_REVERSING: { - throttle_out = rover.channel_throttle->get_control_in(); - steering_out = rover.channel_steer->get_control_in(); + throttle_out = rover.channel_throttle->norm_input_dz() * 100.0f; + steering_out = rover.channel_steer->norm_input_dz() * 4500.0f; break; } } @@ -156,7 +156,7 @@ void Mode::get_pilot_desired_lateral(float &lateral_out) const } // get pilot lateral input - lateral_out = rover.channel_lateral->get_control_in(); + lateral_out = rover.channel_lateral->norm_input_dz() * 100.0f; } // decode pilot's input and return heading_out (in cd) and speed_out (in m/s) diff --git a/Rover/motor_test.cpp b/Rover/motor_test.cpp index eaca91d72cc50..461c0b7152283 100644 --- a/Rover/motor_test.cpp +++ b/Rover/motor_test.cpp @@ -43,7 +43,7 @@ void Rover::motor_test_output() if (motor_test_instance == AP_MotorsUGV::MOTOR_TEST_STEERING) { test_result = g2.motors.output_test_pct(motor_test_instance, channel_steer->norm_input_dz() * 100.0f); } else { - test_result = g2.motors.output_test_pct(motor_test_instance, channel_throttle->get_control_in()); + test_result = g2.motors.output_test_pct(motor_test_instance, channel_throttle->norm_input_dz() * 100.0f); } break; diff --git a/Rover/radio.cpp b/Rover/radio.cpp index 9a2ddee35ef18..a166b5a98f5f6 100644 --- a/Rover/radio.cpp +++ b/Rover/radio.cpp @@ -12,10 +12,10 @@ void Rover::set_control_channels(void) channel_lateral = &rc().get_yaw_channel(); // set rc channel ranges - channel_steer->set_angle(SERVO_MAX); - channel_throttle->set_angle(100); + channel_steer->set_angle(); + channel_throttle->set_angle(); if (channel_lateral != nullptr) { - channel_lateral->set_angle(100); + channel_lateral->set_angle(); } // walking robots rc input init @@ -23,15 +23,15 @@ void Rover::set_control_channels(void) channel_pitch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::PITCH); channel_walking_height = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WALKING_HEIGHT); if (channel_roll != nullptr) { - channel_roll->set_angle(SERVO_MAX); + channel_roll->set_angle(); channel_roll->set_default_dead_zone(30); } if (channel_pitch != nullptr) { - channel_pitch->set_angle(SERVO_MAX); + channel_pitch->set_angle(); channel_pitch->set_default_dead_zone(30); } if (channel_walking_height != nullptr) { - channel_walking_height->set_angle(SERVO_MAX); + channel_walking_height->set_angle(); channel_walking_height->set_default_dead_zone(30); } diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index 2c5caee8d0568..f767aa0861393 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -164,7 +164,7 @@ void Sailboat::init_rc_in() if (rc_ptr != nullptr) { // use aux as sail input if defined channel_mainsail = rc_ptr; - channel_mainsail->set_angle(100); + channel_mainsail->set_angle(); channel_mainsail->set_default_dead_zone(30); } else { // use throttle channel @@ -180,9 +180,9 @@ void Sailboat::set_pilot_desired_mainsail() if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) { relax_sails(); } else { - rover.g2.motors.set_mainsail(constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f)); - rover.g2.motors.set_wingsail(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f)); - rover.g2.motors.set_mast_rotation(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f)); + rover.g2.motors.set_mainsail(constrain_float(channel_mainsail->norm_input_dz() * 100.0f, 0.0f, 100.0f)); + rover.g2.motors.set_wingsail(constrain_float(channel_mainsail->norm_input_dz() * 100.0f, -100.0f, 100.0f)); + rover.g2.motors.set_mast_rotation(constrain_float(channel_mainsail->norm_input_dz() * 100.0f, -100.0f, 100.0f)); } } diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index ab90ba39c6148..85f231935ea62 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -377,6 +377,32 @@ def DriveMaxRCIN(self, timeout=30): self.disarm_vehicle() + def ThrottleLoggingScale(self): + '''Verify THR.ThrIn is logged on a 0..1000 scale''' + self.change_mode("MANUAL") + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(3, 2000) + self.delay_sim_time(2) + self.set_rc(3, 1500) + self.delay_sim_time(1) + self.disarm_vehicle() + + dfreader = self.dfreader_for_current_onboard_log() + max_thr_in = -1 + while True: + m = dfreader.recv_match(type='THR') + if m is None: + break + max_thr_in = max(max_thr_in, m.ThrIn) + + if max_thr_in < 500: + raise NotAchievedException( + "THR.ThrIn max=%d; expected >=500 at full throttle " + "(scale should be 0..1000, not 0..100)" % max_thr_in + ) + self.progress("THR.ThrIn max=%d OK (0..1000 scale)" % max_thr_in) + ################################################# # AUTOTEST ALL ################################################# @@ -7518,6 +7544,7 @@ def tests(self): self.GCSFailsafe, self.RoverInitialMode, self.DriveMaxRCIN, + self.ThrottleLoggingScale, self.NoArmWithoutMissionItems, self.PARAM_ERROR, self.CompassPrearms, diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index bbe2e10e881cf..e3226c0e99b95 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -855,7 +855,7 @@ bool AP_Arming::rc_arm_checks(AP_Arming::Method method) }; for (const auto &channel_to_check : channels_to_check) { const auto *c = channel_to_check.channel; - if (c->get_control_in() != 0) { + if (!is_zero(c->norm_input_dz())) { if ((method != Method::RUDDER) || (c != rc().get_arming_channel())) { // ignore the yaw input channel if rudder arming check_failed(Check::RC, true, "%s (RC%d) is not neutral", channel_to_check.name, c->ch()); check_passed = false; @@ -867,8 +867,8 @@ bool AP_Arming::rc_arm_checks(AP_Arming::Method method) // if throttle check is enabled, require zero input if (rc().arming_check_throttle()) { const RC_Channel *c = &rc().get_throttle_channel(); - if (c->get_control_in() != 0) { - check_failed(Check::RC, true, "%s (RC%d) is not neutral", "Throttle", c->ch()); + if (!is_zero(c->norm_input_dz())) { + check_failed(Check::RC, true, "%s (RC%d) is not neutral (%f)", "Throttle", c->ch(), c->norm_input_dz()); check_passed = false; } c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR); @@ -1958,7 +1958,7 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) } if (method == AP_Arming::Method::RUDDER) { // if throttle is not down, then pilot cannot rudder arm/disarm - if (rc().get_throttle_channel().get_control_in() > 0) { + if (rc().get_throttle_channel().norm_input_dz() > 0.0f) { return false; } // option must be enabled: diff --git a/libraries/AP_Generator/AP_Generator_Loweheiser.cpp b/libraries/AP_Generator/AP_Generator_Loweheiser.cpp index a26e46ea8854a..a350e7053731f 100644 --- a/libraries/AP_Generator/AP_Generator_Loweheiser.cpp +++ b/libraries/AP_Generator/AP_Generator_Loweheiser.cpp @@ -355,7 +355,7 @@ void AP_Generator_Loweheiser::check_rc_input_channels() if (x != rc_channel_manual_throttle) { rc_channel_manual_throttle = x; if (rc_channel_manual_throttle != nullptr) { - rc_channel_manual_throttle->set_range(4500); + rc_channel_manual_throttle->set_range(); } } diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index bf8b1fda1255a..6d36b85934659 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -278,16 +278,14 @@ RC_Channel::RC_Channel(void) AP_Param::setup_object_defaults(this, var_info); } -void RC_Channel::set_range(uint16_t high) +void RC_Channel::set_range() { type_in = ControlType::RANGE; - high_in = high; } -void RC_Channel::set_angle(uint16_t angle) +void RC_Channel::set_angle() { type_in = ControlType::ANGLE; - high_in = angle; } void RC_Channel::set_default_dead_zone(int16_t dzone) @@ -312,117 +310,68 @@ bool RC_Channel::update(void) return false; } + // compute and cache normalised input with deadzone if (type_in == ControlType::RANGE) { - control_in = pwm_to_range(); + // RANGE channels: 0.0 at bottom (radio_min), 1.0 at top (radio_max) + int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); + if (reversed) { + r_in = radio_max + radio_min - r_in; + } + const int16_t dead_zone_top = radio_min + dead_zone; + if (r_in <= dead_zone_top) { + radio_norm = 0.0f; + } else { + radio_norm = constrain_float((float)(r_in - dead_zone_top) / (float)(radio_max - dead_zone_top), 0.0f, 1.0f); + } } else { - // ControlType::ANGLE - control_in = pwm_to_angle(); + // ANGLE channels: 0.0 at trim, -1.0 at min, 1.0 at max + const int16_t dz_min = radio_trim - dead_zone; + const int16_t dz_max = radio_trim + dead_zone; + const float reverse_mul = reversed ? -1.0f : 1.0f; + if (radio_in < dz_min && dz_min > radio_min) { + radio_norm = constrain_float(reverse_mul * (float)(radio_in - dz_min) / (float)(dz_min - radio_min), -1.0f, 1.0f); + } else if (radio_in > dz_max && radio_max > dz_max) { + radio_norm = constrain_float(reverse_mul * (float)(radio_in - dz_max) / (float)(radio_max - dz_max), -1.0f, 1.0f); + } else { + radio_norm = 0.0f; + } } return true; } -/* - return the center stick position expressed as a control_in value - used for thr_mid in copter - */ -int16_t RC_Channel::get_control_mid() const -{ - if (type_in == ControlType::RANGE) { - int16_t r_in = (radio_min.get() + radio_max.get())/2; - - int16_t radio_trim_low = radio_min + dead_zone; - - return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low)); - } else { - return 0; - } -} - -/* - return an "angle in centidegrees" (normally -4500 to 4500) from - the current radio_in value using the specified dead_zone - */ -float RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim) const +// return normalised input [-1, 1] with custom dead_zone and trim, computed live from radio_in +float RC_Channel::norm_input_dz_trim(uint16_t _dead_zone, uint16_t _trim) const { - int16_t radio_trim_high = _trim + _dead_zone; - int16_t radio_trim_low = _trim - _dead_zone; - - float reverse_mul = (reversed?-1:1); - + const int16_t dz_min = _trim - _dead_zone; + const int16_t dz_max = _trim + _dead_zone; + const float reverse_mul = reversed ? -1.0f : 1.0f; // don't allow out of range values - int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); - - if (r_in > radio_trim_high && radio_max != radio_trim_high) { - return reverse_mul * ((float)high_in * (float)(r_in - radio_trim_high)) / (float)(radio_max - radio_trim_high); - } else if (r_in < radio_trim_low && radio_trim_low != radio_min) { - return reverse_mul * ((float)high_in * (float)(r_in - radio_trim_low)) / (float)(radio_trim_low - radio_min); - } else { - return 0; + const int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); + if (r_in < dz_min && dz_min > radio_min) { + return constrain_float(reverse_mul * (float)(r_in - dz_min) / (float)(dz_min - radio_min), -1.0f, 1.0f); + } else if (r_in > dz_max && radio_max > dz_max) { + return constrain_float(reverse_mul * (float)(r_in - dz_max) / (float)(radio_max - dz_max), -1.0f, 1.0f); } -} - -/* - return an "angle in centidegrees" (normally -4500 to 4500) from - the current radio_in value using the specified dead_zone - */ -float RC_Channel::pwm_to_angle_dz(uint16_t _dead_zone) const -{ - return pwm_to_angle_dz_trim(_dead_zone, radio_trim); -} - -/* - return an "angle in centidegrees" (normally -4500 to 4500) from - the current radio_in value - */ -float RC_Channel::pwm_to_angle() const -{ - return pwm_to_angle_dz(dead_zone); -} - - -/* - convert a pulse width modulation value to a value in the configured - range, using the specified deadzone - */ -float RC_Channel::pwm_to_range_dz(uint16_t _dead_zone) const -{ - int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); - - if (reversed) { - r_in = radio_max.get() - (r_in - radio_min.get()); - } - - int16_t radio_trim_low = radio_min + _dead_zone; - - if (r_in > radio_trim_low) { - return (((float)(high_in) * (float)(r_in - radio_trim_low)) / (float)(radio_max - radio_trim_low)); - } - return 0; -} - -/* - convert a pulse width modulation value to a value in the configured - range - */ -float RC_Channel::pwm_to_range() const -{ - return pwm_to_range_dz(dead_zone); -} - - -float RC_Channel::get_control_in_zero_dz(void) const -{ - if (type_in == ControlType::RANGE) { - return pwm_to_range_dz(0); - } - return pwm_to_angle_dz(0); + return 0.0f; } // ------------------------------------------ float RC_Channel::norm_input() const { + if (type_in == ControlType::RANGE) { + // RANGE channels: 0.0 at bottom (radio_min), 1.0 at top (radio_max) + if (radio_max <= radio_min) { + return 0.0f; + } + int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); + if (reversed) { + r_in = radio_max + radio_min - r_in; + } + return constrain_float((float)(r_in - radio_min) / (float)(radio_max - radio_min), 0.0f, 1.0f); + } + // ANGLE channels: 0.0 at trim, -1.0 at min, 1.0 at max float ret; int16_t reverse_mul = (reversed?-1:1); if (radio_in < radio_trim) { @@ -439,22 +388,6 @@ float RC_Channel::norm_input() const return constrain_float(ret, -1.0f, 1.0f); } -float RC_Channel::norm_input_dz() const -{ - int16_t dz_min = radio_trim - dead_zone; - int16_t dz_max = radio_trim + dead_zone; - float ret; - int16_t reverse_mul = (reversed?-1:1); - if (radio_in < dz_min && dz_min > radio_min) { - ret = reverse_mul * (float)(radio_in - dz_min) / (float)(dz_min - radio_min); - } else if (radio_in > dz_max && radio_max > dz_max) { - ret = reverse_mul * (float)(radio_in - dz_max) / (float)(radio_max - dz_max); - } else { - ret = 0; - } - return constrain_float(ret, -1.0f, 1.0f); -} - // return a normalised input for a channel, in range -1 to 1, // ignores trim and deadzone float RC_Channel::norm_input_ignore_trim() const @@ -594,7 +527,7 @@ float RC_Channel::stick_mixing(const float servo_in) float servo_out = servo_in; servo_out *= ch_inf; - servo_out += control_in; + servo_out += radio_norm * 4500.0f; return servo_out; } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 545916c8afc37..bb339d3f15407 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -49,30 +49,27 @@ class RC_Channel { uint8_t ch() const { return ch_in + 1; } // setup the control preferences - void set_range(uint16_t high); - uint16_t get_range() const { return high_in; } - void set_angle(uint16_t angle); + void set_range(); + void set_angle(); bool get_reverse(void) const; void set_default_dead_zone(int16_t dzone); uint16_t get_dead_zone(void) const { return dead_zone; } - // get the center stick position expressed as a control_in value - int16_t get_control_mid() const; - - // read input from hal.rcin - create a control_in value + // read input from hal.rcin bool update(void); - // calculate an angle given dead_zone and trim. This is used by the quadplane code - // for hover throttle - float pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim) const; - // return a normalised input for a channel, in range -1 to 1, // centered around the channel trim. Ignore deadzone. float norm_input() const; // return a normalised input for a channel, in range -1 to 1, - // centered around the channel trim. Take into account the deadzone - float norm_input_dz() const; + // centered around the channel trim. Take into account the deadzone. + // Returns the cached value from update(); can be overridden with set_norm_in(). + float norm_input_dz() const { return radio_norm; } + + // return a normalised input for a channel, in range -1 to 1, centered + // around the given trim with the given dead_zone. Does not use the cache. + float norm_input_dz_trim(uint16_t dead_zone, uint16_t trim) const; // return a normalised input for a channel, in range -1 to 1, // ignores trim and deadzone @@ -96,8 +93,8 @@ class RC_Channel { int16_t get_radio_in() const { return radio_in;} void set_radio_in(int16_t val) {radio_in = val;} - int16_t get_control_in() const { return control_in;} - void set_control_in(int16_t val) { control_in = val;} + // override the normalised input (e.g. for failsafe or frame-rotation). Value in [-1, 1]. + void set_norm_in(float val) { radio_norm = val; } void clear_override(); void set_override(const uint16_t v, const uint32_t timestamp_ms); @@ -105,9 +102,6 @@ class RC_Channel { float stick_mixing(const float servo_in); - // get control input with zero deadzone - float get_control_in_zero_dz(void) const; - int16_t get_radio_min() const {return radio_min.get();} int16_t get_radio_max() const {return radio_max.get();} @@ -530,8 +524,8 @@ class RC_Channel { // stores the raw pwm value read from hal.rcin int16_t raw_radio_in; - // value generated from PWM normalised to configured scale - int16_t control_in; + // normalised input in [-1, 1], updated by update(), overridable with set_norm_in() + float radio_norm; AP_Int16 radio_min; AP_Int16 radio_trim; @@ -541,7 +535,6 @@ class RC_Channel { AP_Int16 dead_zone; ControlType type_in; - int16_t high_in; // overrides uint16_t override_value; @@ -551,12 +544,6 @@ class RC_Channel { bool in_raw_trim_dz() const; int16_t get_raw_radio_in() const { return raw_radio_in; } - float pwm_to_angle() const; - float pwm_to_angle_dz(uint16_t dead_zone) const; - - float pwm_to_range() const; - float pwm_to_range_dz(uint16_t dead_zone) const; - bool read_3pos_switch(AuxSwitchPos &ret) const WARN_IF_UNUSED; bool read_6pos_switch(int8_t& position) WARN_IF_UNUSED; diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 7f6d392154336..230def82b3258 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -467,25 +467,25 @@ void RC_Channels::rudder_arm_disarm_check() return; } - const auto control_in = channel->get_control_in(); - const auto abs_control_in = abs(control_in); + const float norm_in = channel->norm_input_dz(); + const float abs_norm_in = fabsf(norm_in); - if (abs_control_in == 0) { + if (is_zero(abs_norm_in)) { have_seen_neutral_rudder = true; } - if (abs_control_in <= 4000) { + if (abs_norm_in <= 4000.0f/4500.0f) { // not trying to (or no longer trying to) arm or disarm rudder_arm_timer = 0; return; } // enforce correct stick gesture for arming (but not disarming): - if (arming_check_throttle() && control_in > 4000) { + if (arming_check_throttle() && norm_in > 4000.0f/4500.0f) { // only permit arming if the vehicle isn't being commanded to // move via RC input const auto &c = rc().get_throttle_channel(); - if (c.get_control_in() != 0) { + if (!is_zero(c.norm_input_dz())) { rudder_arm_timer = 0; return; } @@ -505,7 +505,7 @@ void RC_Channels::rudder_arm_disarm_check() // time to try to arm or disarm: rudder_arm_timer = 0; - if (control_in > 4000) { + if (norm_in > 4000.0f/4500.0f) { AP::arming().arm(AP_Arming::Method::RUDDER); have_seen_neutral_rudder = false; } else { diff --git a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.cpp index bbc2bf558daa8..daf1b578038ba 100644 --- a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.cpp @@ -73,25 +73,25 @@ void setup() print_radio_values(); // set type of output, symmetrical angles or a number range; - rc().channel(CH_1)->set_angle(4500); + rc().channel(CH_1)->set_angle(); rc().channel(CH_1)->set_default_dead_zone(80); - rc().channel(CH_2)->set_angle(4500); + rc().channel(CH_2)->set_angle(); rc().channel(CH_2)->set_default_dead_zone(80); - rc().channel(CH_3)->set_range(1000); + rc().channel(CH_3)->set_range(); rc().channel(CH_3)->set_default_dead_zone(20); - rc().channel(CH_4)->set_angle(6000); + rc().channel(CH_4)->set_angle(); rc().channel(CH_4)->set_default_dead_zone(500); - rc().channel(CH_5)->set_range(1000); + rc().channel(CH_5)->set_range(); - rc().channel(CH_6)->set_range(800); + rc().channel(CH_6)->set_range(); - rc().channel(CH_7)->set_range(1000); + rc().channel(CH_7)->set_range(); - rc().channel(CH_8)->set_range(1000); + rc().channel(CH_8)->set_range(); } void loop() @@ -107,7 +107,7 @@ void loop() rc().read_input(); for (uint8_t i=0; iprintf("%5d ", (int)rc().channel(i)->get_control_in()); + hal.console->printf("%6.3f ", rc().channel(i)->norm_input_dz()); // hal.console->printf("%4d ", (int)rc().channel(i)->percent_input()); } hal.console->printf("\n"); diff --git a/libraries/RC_Channel/tests/test_range_normalisation.cpp b/libraries/RC_Channel/tests/test_range_normalisation.cpp new file mode 100644 index 0000000000000..2a1a7c81cfca0 --- /dev/null +++ b/libraries/RC_Channel/tests/test_range_normalisation.cpp @@ -0,0 +1,294 @@ +/* + Stand-alone tests for RC_Channel RANGE input normalisation. + + Two areas are covered: + + 1. pwm_to_range_dz() / get_control_in() equivalence + The old get_control_in() for RANGE channels returned + int16_t(pwm_to_range_dz(dead_zone) * 1000) + The replacement is + int16_t(norm_input_dz() * 1000) + Both implementations are inlined here so the test has no external + dependencies beyond AP_Math and gtest. + + 2. get_throttle_mid() correctness + The original get_control_mid() used the geometric midpoint + (radio_min + radio_max) / 2 as the reference PWM value. + A bug introduced during the normalisation refactor substituted + radio_trim for that midpoint, shifting the throttle-expo centre + for any pilot whose trim was not exactly at the geometric centre. + The fixed version and the buggy version are both inlined so the + test can demonstrate both the correct behaviour and the regression. +*/ + +#include +#include +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +// --------------------------------------------------------------------------- +// Standalone implementations — no RC_Channel object required +// --------------------------------------------------------------------------- + +/* + * Old pwm_to_range_dz() from master:libraries/RC_Channel/RC_Channel.cpp. + * Returns [0, 1]: 0 at (radio_min + dead_zone), 1 at radio_max. + */ +static float pwm_to_range_dz(uint16_t radio_in, uint16_t radio_min, uint16_t radio_max, + uint16_t dead_zone) +{ + int16_t r_in = constrain_int16(int16_t(radio_in), int16_t(radio_min), int16_t(radio_max)); + float in_min = float(radio_min); + if (dead_zone > 0) { + if (radio_max - radio_min <= 2 * dead_zone) { + return (r_in > (radio_max + radio_min) / 2) ? 1.0f : 0.0f; + } + in_min += float(dead_zone); + } + return constrain_float((float(r_in) - in_min) / (float(radio_max) - in_min), 0.0f, 1.0f); +} + +/* + * Old get_control_in() for RANGE: pwm_to_range_dz * high_in, high_in = 1000. + */ +static int16_t get_control_in_range(uint16_t radio_in, uint16_t radio_min, uint16_t radio_max, + uint16_t dead_zone) +{ + return int16_t(pwm_to_range_dz(radio_in, radio_min, radio_max, dead_zone) * 1000.0f); +} + +/* + * New norm_input_dz() for RANGE channels, from RC_Channel::update(). + * Identical semantics to pwm_to_range_dz but written without high_in. + */ +static float norm_input_dz_range(uint16_t radio_in, uint16_t radio_min, uint16_t radio_max, + uint16_t dead_zone) +{ + const int16_t r_in = constrain_int16(int16_t(radio_in), int16_t(radio_min), int16_t(radio_max)); + const int16_t dead_zone_top = int16_t(radio_min) + int16_t(dead_zone); + if (r_in <= dead_zone_top) { + return 0.0f; + } + return constrain_float(float(r_in - dead_zone_top) / float(int16_t(radio_max) - dead_zone_top), + 0.0f, 1.0f); +} + +/* + * Original get_control_mid() from master:RC_Channel.cpp. + * Uses the geometric midpoint (radio_min + radio_max) / 2 as reference PWM. + * Returns a value in [0, 1000] representing where that midpoint sits in the + * normalised throttle range. + */ +static int16_t get_control_mid(uint16_t radio_min, uint16_t radio_max, uint16_t dead_zone) +{ + const int16_t r_in = int16_t((radio_min + radio_max) / 2); + const int16_t radio_trim_low = int16_t(radio_min) + int16_t(dead_zone); + return int16_t(((int32_t)1000 * int32_t(r_in - radio_trim_low)) + / int32_t(int16_t(radio_max) - radio_trim_low)); +} + +/* + * Fixed get_throttle_mid() from ArduCopter/radio.cpp (after the bug fix). + * Also uses the geometric midpoint — should match get_control_mid() exactly. + */ +static int16_t get_throttle_mid_fixed(uint16_t radio_min, uint16_t radio_max, uint16_t dead_zone) +{ + const int16_t r_min = int16_t(radio_min); + const int16_t r_max = int16_t(radio_max); + const int16_t r_mid = (r_min + r_max) / 2; + const int16_t dead_zone_top = r_min + int16_t(dead_zone); + if (dead_zone_top >= r_max || r_mid <= dead_zone_top) { + return 500; + } + return int16_t(1000.0f * float(r_mid - dead_zone_top) / float(r_max - dead_zone_top)); +} + +/* + * Buggy get_throttle_mid() — the version that was introduced during the + * normalisation refactor. Uses radio_trim instead of the geometric midpoint, + * so the expo-curve centre drifts whenever the pilot's trim is not exactly + * at (radio_min + radio_max) / 2. + */ +static int16_t get_throttle_mid_buggy(uint16_t radio_min, uint16_t radio_max, + uint16_t radio_trim, uint16_t dead_zone) +{ + const int16_t r_max = int16_t(radio_max); + const int16_t r_trim = int16_t(radio_trim); + const int16_t dead_zone_top = int16_t(radio_min) + int16_t(dead_zone); + if (dead_zone_top >= r_max || r_trim <= dead_zone_top) { + return 500; + } + return int16_t(1000.0f * float(r_trim - dead_zone_top) / float(r_max - dead_zone_top)); +} + +// --------------------------------------------------------------------------- +// Test parameters +// --------------------------------------------------------------------------- + +struct RCConfig { + uint16_t radio_min; + uint16_t radio_max; + uint16_t radio_trim; // used only by buggy variant + uint16_t dead_zone; + const char *label; +}; + +static const RCConfig configs[] = { + // radio_min radio_max radio_trim dead_zone label + { 1000, 2000, 1500, 30, "typical centred" }, + { 982, 2006, 1500, 30, "wide range centred trim" }, + { 988, 2012, 1500, 30, "common asymmetric range" }, + { 1000, 2000, 1500, 0, "no dead zone" }, + { 1000, 2000, 1500, 100, "large dead zone" }, + { 1000, 1800, 1400, 30, "short throw" }, + // Trim offset cases — these expose the get_throttle_mid bug: + { 1000, 2000, 1200, 30, "trim 300 below centre" }, + { 1000, 2000, 1700, 30, "trim 200 above centre" }, + { 1000, 2000, 1000, 30, "trim at minimum (heli style)" }, + { 1000, 2000, 2000, 30, "trim at maximum" }, + // Wider real-world range with off-centre trim: + { 982, 2006, 1100, 30, "wide range, low trim" }, + { 982, 2006, 1900, 30, "wide range, high trim" }, +}; + +// --------------------------------------------------------------------------- +// Helper +// --------------------------------------------------------------------------- + +// The two integer implementations use different rounding (floor integer +// division vs float truncation); allow ±1 count tolerance. +static constexpr int16_t MID_TOLERANCE = 1; + +// --------------------------------------------------------------------------- +// Tests: pwm_to_range_dz vs norm_input_dz +// --------------------------------------------------------------------------- + +// norm_input_dz() for RANGE must give the same [0,1000] reading as the old +// get_control_in() across the full stick travel. +TEST(RCRangeNorm, EquivalentToOldGetControlIn) +{ + for (const auto& c : configs) { + for (uint16_t pwm = c.radio_min; pwm <= c.radio_max; pwm += 5) { + const int16_t old_val = get_control_in_range(pwm, c.radio_min, c.radio_max, c.dead_zone); + const int16_t new_val = int16_t(norm_input_dz_range(pwm, c.radio_min, c.radio_max, c.dead_zone) * 1000.0f); + EXPECT_EQ(old_val, new_val) + << c.label + << " min=" << c.radio_min << " max=" << c.radio_max + << " dz=" << c.dead_zone << " pwm=" << pwm; + } + } +} + +// At stick bottom the output must be zero (deadzone clamps it). +TEST(RCRangeNorm, BottomStickIsZero) +{ + for (const auto& c : configs) { + EXPECT_EQ(0, int16_t(norm_input_dz_range(c.radio_min, c.radio_min, c.radio_max, c.dead_zone) * 1000.0f)) + << c.label; + } +} + +// At stick top the output must be 1000. +TEST(RCRangeNorm, TopStickIs1000) +{ + for (const auto& c : configs) { + EXPECT_EQ(1000, int16_t(norm_input_dz_range(c.radio_max, c.radio_min, c.radio_max, c.dead_zone) * 1000.0f)) + << c.label; + } +} + +// Output must be non-decreasing as PWM increases. +TEST(RCRangeNorm, Monotonic) +{ + for (const auto& c : configs) { + int16_t prev = 0; + for (uint16_t pwm = c.radio_min; pwm <= c.radio_max; pwm += 5) { + const int16_t val = int16_t(norm_input_dz_range(pwm, c.radio_min, c.radio_max, c.dead_zone) * 1000.0f); + EXPECT_GE(val, prev) << c.label << " pwm=" << pwm; + prev = val; + } + } +} + +// --------------------------------------------------------------------------- +// Tests: get_throttle_mid +// --------------------------------------------------------------------------- + +// Fixed get_throttle_mid() must match the original get_control_mid() for all +// configs. Note: get_control_mid uses integer division; get_throttle_mid_fixed +// uses float division — allow ±1 for rounding differences. +TEST(ThrottleMid, FixedMatchesOriginalGetControlMid) +{ + for (const auto& c : configs) { + const int16_t original = get_control_mid(c.radio_min, c.radio_max, c.dead_zone); + const int16_t fixed = get_throttle_mid_fixed(c.radio_min, c.radio_max, c.dead_zone); + EXPECT_NEAR(original, fixed, MID_TOLERANCE) + << c.label + << " min=" << c.radio_min << " max=" << c.radio_max + << " dz=" << c.dead_zone; + } +} + +// Midpoint must always lie within [0, 1000]. +TEST(ThrottleMid, OutputInValidRange) +{ + for (const auto& c : configs) { + const int16_t mid = get_throttle_mid_fixed(c.radio_min, c.radio_max, c.dead_zone); + EXPECT_GE(mid, 0) << c.label; + EXPECT_LE(mid, 1000) << c.label; + } +} + +// Symmetric range with no dead zone: midpoint must be exactly 500. +TEST(ThrottleMid, SymmetricNoDzGives500) +{ + EXPECT_EQ(500, get_throttle_mid_fixed(1000, 2000, 0)); + EXPECT_EQ(500, get_throttle_mid_fixed( 800, 1800, 0)); + EXPECT_EQ(500, get_throttle_mid_fixed(1100, 1900, 0)); +} + +// The buggy variant agrees with the correct one only when radio_trim equals +// the geometric centre of the PWM range. When trim is offset the buggy +// variant produces a different (wrong) midpoint. +TEST(ThrottleMid, BuggyVariantDiffersWhenTrimIsOffCentre) +{ + for (const auto& c : configs) { + const int16_t correct = get_throttle_mid_fixed(c.radio_min, c.radio_max, c.dead_zone); + const int16_t buggy = get_throttle_mid_buggy(c.radio_min, c.radio_max, + c.radio_trim, c.dead_zone); + const int16_t geometric_centre = int16_t((c.radio_min + c.radio_max) / 2); + + if (c.radio_trim == geometric_centre) { + // When trim is at centre both versions must agree. + EXPECT_NEAR(correct, buggy, MID_TOLERANCE) + << c.label << " (trim at centre — both variants should agree)"; + } else { + // When trim is off-centre the buggy variant drifts from the + // correct midpoint. The difference is proportional to how far + // trim sits from (radio_min + radio_max) / 2. + EXPECT_NE(correct, buggy) + << c.label + << " (trim=" << c.radio_trim << " != centre " << geometric_centre + << " — buggy variant should give wrong midpoint)"; + } + } +} + +// Quantify the error introduced by the bug for a concrete case: +// min=1000 max=2000 trim=1700 dz=30 +// correct: based on geometric centre 1500 → ~484 +// buggy: based on trim 1700 → ~690 +// That is a 206-count error on a 0-1000 scale (~20% of full range). +TEST(ThrottleMid, BugMagnitudeExample) +{ + const int16_t correct = get_throttle_mid_fixed(1000, 2000, 30); + const int16_t buggy = get_throttle_mid_buggy(1000, 2000, 1700, 30); + EXPECT_NEAR(correct, 484, 1); + EXPECT_NEAR(buggy, 690, 1); + EXPECT_GT(std::abs(correct - buggy), 100) + << "bug should produce a large midpoint shift, got correct=" + << correct << " buggy=" << buggy; +} + +AP_GTEST_MAIN() diff --git a/libraries/RC_Channel/tests/wscript b/libraries/RC_Channel/tests/wscript new file mode 100644 index 0000000000000..e0905f27bea57 --- /dev/null +++ b/libraries/RC_Channel/tests/wscript @@ -0,0 +1,4 @@ +#!/usr/bin/env python3 + +def build(bld): + bld.ap_find_tests(use='ap')