From 0266168e69e28ffd1fadb9a1edd7deb89ae6987e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Jul 2017 14:14:00 +1000 Subject: [PATCH] Plane: removed old elevon and vtail mixers now only use the function based mixing, and auto-convert on startup --- ArduPlane/Parameters.cpp | 128 ++++++++++++++++++++++++++++++++++----- ArduPlane/Parameters.h | 6 +- ArduPlane/Plane.h | 1 + ArduPlane/afs_plane.cpp | 2 - ArduPlane/radio.cpp | 21 ++----- ArduPlane/servos.cpp | 12 +--- 6 files changed, 122 insertions(+), 48 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index a8b71176b0..95ccb06386 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -692,20 +692,6 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard GSCALAR(auto_trim, "TRIM_AUTO", AUTO_TRIM), - // @Param: VTAIL_OUTPUT - // @DisplayName: VTail output - // @Description: Enable VTail output in software. If enabled then the APM will provide software VTail mixing on the elevator and rudder channels. There are 8 different mixing modes available, which refer to the 8 ways the elevator can be mapped to the two VTail servos. Please also see the MIXING_GAIN parameter for the output gain of the mixer. - // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown,5:UpUpSwap,6:UpDownSwap,7:DownUpSwap,8:DownDownSwap - // @User: User - GSCALAR(vtail_output, "VTAIL_OUTPUT", 0), - - // @Param: ELEVON_OUTPUT - // @DisplayName: Elevon output - // @Description: Enable software elevon output mixer. If enabled then the APM will provide software elevon mixing on the aileron and elevator channels. There are 8 different mixing modes available, which refer to the 8 ways the elevator can be mapped to the two elevon servos. Please also see the MIXING_GAIN parameter for the output gain of the mixer. - // @Values: 0:Disabled,1:UpUp,2:UpDown,3:DownUp,4:DownDown,5:UpUpSwap,6:UpDownSwap,7:DownUpSwap,8:DownDownSwap - // @User: User - GSCALAR(elevon_output, "ELEVON_OUTPUT", 0), - // @Param: MIXING_GAIN // @DisplayName: Mixing Gain // @Description: The gain for the Vtail and elevon output mixers. The default is 0.5, which ensures that the mixer doesn't saturate, allowing both input channels to go to extremes while retaining control over the output. Hardware mixers often have a 1.0 gain, which gives more servo throw, but can saturate. If you don't have enough throw on your servos with VTAIL_OUTPUT or ELEVON_OUTPUT enabled then you can raise the gain using MIXING_GAIN. The mixer allows outputs in the range 900 to 2100 microseconds. @@ -1316,6 +1302,9 @@ void Plane::load_parameters(void) const uint16_t old_aux_chan_mask = 0x3FF0; SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap); + // possibly convert elevon and vtail mixers + convert_mixers(); + if (quadplane.enable) { // quadplanes needs a higher loop rate AP_Param::set_default_by_name("SCHED_LOOP_RATE", 300); @@ -1325,3 +1314,114 @@ void Plane::load_parameters(void) cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); } + +/* + convert from old ELEVON_OUTPUT and VTAIL_OUTPUT mixers to function + based mixing + */ +void Plane::convert_mixers(void) +{ + AP_Int8 elevon_output; + AP_Int8 vtail_output; + AP_Param::ConversionInfo elevon_info = { + Parameters::k_param_elevon_output, + 0, + AP_PARAM_INT8, + nullptr + }; + AP_Param::ConversionInfo vtail_info = { + Parameters::k_param_vtail_output, + 0, + AP_PARAM_INT8, + nullptr + }; + SRV_Channel *chan1 = SRV_Channels::srv_channel(CH_1); + SRV_Channel *chan2 = SRV_Channels::srv_channel(CH_2); + SRV_Channel *chan4 = SRV_Channels::srv_channel(CH_4); + + if (AP_Param::find_old_parameter(&vtail_info, &vtail_output) && + vtail_output.get() != 0 && + chan2->get_function() == SRV_Channel::k_elevator && + chan4->get_function() == SRV_Channel::k_rudder && + !chan2->function_configured() && + !chan4->function_configured()) { + cliSerial->printf("Converting vtail_output %u\n", vtail_output.get()); + switch (vtail_output) { + case MIXING_UPUP: + case MIXING_UPUP_SWP: + chan2->reversed_set_and_save_ifchanged(false); + chan4->reversed_set_and_save_ifchanged(false); + break; + case MIXING_UPDN: + case MIXING_UPDN_SWP: + chan2->reversed_set_and_save_ifchanged(false); + chan4->reversed_set_and_save_ifchanged(true); + break; + case MIXING_DNUP: + case MIXING_DNUP_SWP: + chan2->reversed_set_and_save_ifchanged(true); + chan4->reversed_set_and_save_ifchanged(false); + break; + case MIXING_DNDN: + case MIXING_DNDN_SWP: + chan2->reversed_set_and_save_ifchanged(true); + chan4->reversed_set_and_save_ifchanged(true); + break; + } + if (vtail_output < MIXING_UPUP_SWP) { + chan2->function_set_and_save(SRV_Channel::k_vtail_right); + chan4->function_set_and_save(SRV_Channel::k_vtail_left); + } else { + chan2->function_set_and_save(SRV_Channel::k_vtail_left); + chan4->function_set_and_save(SRV_Channel::k_vtail_right); + } + } else if (AP_Param::find_old_parameter(&elevon_info, &elevon_output) && + elevon_output.get() != 0 && + chan1->get_function() == SRV_Channel::k_aileron && + chan2->get_function() == SRV_Channel::k_elevator && + !chan1->function_configured() && + !chan2->function_configured()) { + cliSerial->printf("convert elevon_output %u\n", elevon_output.get()); + switch (elevon_output) { + case MIXING_UPUP: + case MIXING_UPUP_SWP: + chan2->reversed_set_and_save_ifchanged(false); + chan1->reversed_set_and_save_ifchanged(false); + break; + case MIXING_UPDN: + case MIXING_UPDN_SWP: + chan2->reversed_set_and_save_ifchanged(false); + chan1->reversed_set_and_save_ifchanged(true); + break; + case MIXING_DNUP: + case MIXING_DNUP_SWP: + chan2->reversed_set_and_save_ifchanged(true); + chan1->reversed_set_and_save_ifchanged(false); + break; + case MIXING_DNDN: + case MIXING_DNDN_SWP: + chan2->reversed_set_and_save_ifchanged(true); + chan1->reversed_set_and_save_ifchanged(true); + break; + } + if (elevon_output < MIXING_UPUP_SWP) { + chan1->function_set_and_save(SRV_Channel::k_elevon_right); + chan2->function_set_and_save(SRV_Channel::k_elevon_left); + } else { + chan1->function_set_and_save(SRV_Channel::k_elevon_left); + chan2->function_set_and_save(SRV_Channel::k_elevon_right); + } + } + + // convert any k_aileron_with_input to aileron and k_elevator_with_input to k_elevator + for (uint8_t i=0; iget_function() == SRV_Channel::k_aileron_with_input) { + chan->function_set_and_save(SRV_Channel::k_aileron); + } + if (chan->get_function() == SRV_Channel::k_elevator_with_input) { + chan->function_set_and_save(SRV_Channel::k_elevator); + } + } + +} diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 11306d17ab..072cddd2bb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -86,9 +86,9 @@ public: k_param_takeoff_heading_hold, // unused k_param_level_roll_limit, k_param_hil_servos, - k_param_vtail_output, + k_param_vtail_output, // unused k_param_nav_controller, - k_param_elevon_output, + k_param_elevon_output, // unused k_param_att_controller, k_param_mixing_gain, k_param_scheduler, @@ -458,8 +458,6 @@ public: // Misc // AP_Int8 auto_trim; - AP_Int8 vtail_output; - AP_Int8 elevon_output; AP_Int8 rudder_only; AP_Float mixing_gain; AP_Int16 mixing_offset; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3b3d9ae687..f1bef136ea 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -849,6 +849,7 @@ private: void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page); void load_parameters(void); + void convert_mixers(void); void adjust_altitude_target(); void setup_glide_slope(void); int32_t get_RTL_altitude(); diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index a9fc46da47..6083d18e25 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -23,7 +23,6 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator_with_input, SERVO_MAX); SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); @@ -43,7 +42,6 @@ void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); - SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator_with_input, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index d18416714f..9c931b6af5 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -307,23 +307,10 @@ void Plane::trim_control_surfaces() return; } - // 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); - } - - // 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 main surfaces + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_aileron); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevator); + SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_rudder); // trim elevons SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_left); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 42c5820824..03ad6490aa 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -558,20 +558,10 @@ void Plane::set_servos_flaps(void) */ void Plane::servo_output_mixers(void) { - if (g.vtail_output != MIXING_DISABLED) { - channel_output_mixer(g.vtail_output, SRV_Channel::k_elevator, SRV_Channel::k_rudder); - } else if (g.elevon_output != MIXING_DISABLED) { - channel_output_mixer(g.elevon_output, SRV_Channel::k_elevator, SRV_Channel::k_aileron); - } - - // allow for extra elevon and vtail channels + // mix elevons and vtail channels channel_function_mixer(SRV_Channel::k_aileron, SRV_Channel::k_elevator, SRV_Channel::k_elevon_left, SRV_Channel::k_elevon_right); channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left); - // copy aileron to deprecated aileron_with_input and elevator to deprecated elevator_with_input - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator_with_input, SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)); - // implement differential spoilers dspoiler_update(); }