From 725244ff8af9d9cbbbd20bd831123fde38289b2f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 1 Jul 2017 20:05:17 +1000 Subject: [PATCH] Plane: implement new TRIM_AUTO functionality this fixes TRIM_AUTO for new scaled output approach to servo functions --- ArduPlane/Parameters.cpp | 2 +- ArduPlane/radio.cpp | 77 ++++++++++++++++++++++------------------ 2 files changed, 44 insertions(+), 35 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 6593d8f175..2072233d92 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -687,7 +687,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TRIM_AUTO // @DisplayName: Automatic trim adjustment - // @Description: Set RC trim PWM levels to current levels when switching away from manual mode. When this option is enabled and you change from MANUAL to any other mode then the APM will take the current position of the control sticks as the trim values for aileron, elevator and rudder. It will use those to set RC1_TRIM, RC2_TRIM and RC4_TRIM. This option is disabled by default as if a pilot is not aware of this option and changes from MANUAL to another mode while control inputs are not centered then the trim could be changed to a dangerously bad value. You can enable this option to assist with trimming your plane, by enabling it before takeoff then switching briefly to MANUAL in flight, and seeing how the plane reacts. You can then switch back to FBWA, trim the surfaces then again test MANUAL mode. Each time you switch from MANUAL the APM will take your control inputs as the new trim. After you have good trim on your aircraft you can disable TRIM_AUTO for future flights. + // @Description: Set RC trim PWM levels to current levels when switching away from manual mode. When this option is enabled and you change from MANUAL to any other mode then the APM will take the current position of the control sticks as the trim values for aileron, elevator and rudder. It will use those to set the SERVOn_TRIM values and the RCn_TRIM values. This option is disabled by default as if a pilot is not aware of this option and changes from MANUAL to another mode while control inputs are not centered then the trim could be changed to a dangerously bad value. You can enable this option to assist with trimming your plane, by enabling it before takeoff then switching briefly to MANUAL in flight, and seeing how the plane reacts. You can then switch back to FBWA, trim the surfaces then again test MANUAL mode. Each time you switch from MANUAL the APM will take your control inputs as the new trim. After you have good trim on your aircraft you can disable TRIM_AUTO for future flights. You should also see the newer and much safer SERVO_AUTO_TRIM parameter. // @Values: 0:Disabled,1:Enabled // @User: Standard GSCALAR(auto_trim, "TRIM_AUTO", AUTO_TRIM), diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index b4e3bb07d9..941a774ae7 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -314,42 +314,51 @@ void Plane::trim_control_surfaces() return; } - // Store control surface trim values - // --------------------------------- - if(g.mix_mode == 0) { - if (channel_roll->get_radio_in() != 0) { - channel_roll->set_radio_trim(channel_roll->get_radio_in()); - } - if (channel_pitch->get_radio_in() != 0) { - channel_pitch->set_radio_trim(channel_pitch->get_radio_in()); - } - - // the secondary aileron/elevator is trimmed only if it has a - // corresponding transmitter input channel, which k_aileron - // doesn't have - SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::k_aileron_with_input); - SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::k_elevator_with_input); - } else{ - if (elevon.ch1_temp != 0) { - elevon.trim1 = elevon.ch1_temp; - } - if (elevon.ch2_temp != 0) { - elevon.trim2 = elevon.ch2_temp; - } - //Recompute values here using new values for elevon1_trim and elevon2_trim - //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed - uint16_t center = 1500; - channel_roll->set_radio_trim(center); - channel_pitch->set_radio_trim(center); - } - if (channel_rudder->get_radio_in() != 0) { - channel_rudder->set_radio_trim(channel_rudder->get_radio_in()); + // trim ailerons if not used as old elevons + if (g.elevon_output == MIXING_DISABLED) { + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_aileron); } - // save to eeprom - channel_roll->save_eeprom(); - channel_pitch->save_eeprom(); - channel_rudder->save_eeprom(); + // trim elevator if not used as old elevons or vtail + if (g.elevon_output == MIXING_DISABLED && g.vtail_output == MIXING_DISABLED) { + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevator); + } + + // trim rudder if not used as old vtail + if (g.vtail_output == MIXING_DISABLED) { + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_rudder); + } + + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_aileron_with_input); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevator_with_input); + + // trim elevons + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_left); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_right); + + // trim vtail + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_left); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_right); + + if (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) == 0) { + // trim differential spoilers if no rudder input + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft1); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft2); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight1); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight1); + } + + if (SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) == 0 && + SRV_Channels::get_output_scaled(SRV_Channel::k_flap) == 0) { + // trim flaperons if no flap input + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right); + } + + // now save input trims, as these have been moved to the outputs + channel_roll->set_and_save_trim(); + channel_pitch->set_and_save_trim(); + channel_rudder->set_and_save_trim(); } void Plane::trim_radio()