From a79359dc6748f35a50a38c38b3efdd4f87395d4c Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 23 Jan 2022 22:21:18 +0000 Subject: [PATCH] Plane: use new weathervane library --- ArduPlane/RC_Channel.cpp | 18 +++++++++ ArduPlane/quadplane.cpp | 82 ++++++++++++++++------------------------ ArduPlane/quadplane.h | 10 ++--- 3 files changed, 53 insertions(+), 57 deletions(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 585d44c3bd..dcf71e5085 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -175,6 +175,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, #if HAL_QUADPLANE_ENABLED case AUX_FUNC::Q_ASSIST: case AUX_FUNC::AIRMODE: + case AUX_FUNC::WEATHER_VANE_ENABLE: #endif #if AP_AIRSPEED_AUTOCAL_ENABLE case AUX_FUNC::ARSPD_CALIBRATE: @@ -359,6 +360,23 @@ case AUX_FUNC::ARSPD_CALIBRATE: plane.quadplane.air_mode = AirMode::ON; } break; + + case AUX_FUNC::WEATHER_VANE_ENABLE: { + if (plane.quadplane.weathervane != nullptr) { + switch (ch_flag) { + case AuxSwitchPos::HIGH: + plane.quadplane.weathervane->allow_weathervaning(true); + break; + case AuxSwitchPos::MIDDLE: + // nothing + break; + case AuxSwitchPos::LOW: + plane.quadplane.weathervane->allow_weathervaning(false); + break; + } + } + break; + } #endif case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC: diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 4c7d59d343..7c2566a4ce 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -172,21 +172,9 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("VFWD_GAIN", 32, QuadPlane, vel_forward.gain, 0), - // @Param: WVANE_GAIN - // @DisplayName: Weathervaning gain - // @Description: This controls the tendency to yaw to face into the wind. A value of 0.1 is to start with and will give a slow turn into the wind. Use a value of 0.4 for more rapid response. The weathervaning works by turning into the direction of roll. - // @Range: 0 1 - // @Increment: 0.01 - // @User: Standard - AP_GROUPINFO("WVANE_GAIN", 33, QuadPlane, weathervane.gain, 0), + // 33 was used by WVANE_GAIN - // @Param: WVANE_MINROLL - // @DisplayName: Weathervaning min roll - // @Description: This set the minimum roll in degrees before active weathervaning will start. This may need to be larger if your aircraft has bad roll trim. - // @Range: 0 10 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("WVANE_MINROLL", 34, QuadPlane, weathervane.min_roll, 1), + // 34 was used by WVANE_MINROLL // @Param: RTL_ALT // @DisplayName: QRTL return altitude @@ -442,6 +430,10 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Values: -1:Warn only, 0:QLand, 1:QRTL AP_GROUPINFO("TRANS_FAIL_ACT", 29, QuadPlane, transition_failure.action, 0), + // @Group: WVANE_ + // @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp + AP_SUBGROUPPTR(weathervane, "WVANE_", 30, QuadPlane, AC_WeatherVane), + AP_GROUPEND }; @@ -527,6 +519,10 @@ const AP_Param::ConversionInfo q_conversion_table[] = { { Parameters::k_param_quadplane, 22, AP_PARAM_INT16, "Q_M_PWM_MIN" }, { Parameters::k_param_quadplane, 23, AP_PARAM_INT16, "Q_M_PWM_MAX" }, + + // PARAMETER_CONVERSION - Added: Jan-2022 + { Parameters::k_param_quadplane, 33, AP_PARAM_FLOAT, "Q_WVANE_GAIN" }, // Moved from quadplane to weathervane library + { Parameters::k_param_quadplane, 34, AP_PARAM_FLOAT, "Q_WVANE_ANG_MIN" }, // Q_WVANE_MINROLL moved from quadplane to weathervane library }; @@ -591,7 +587,7 @@ bool QuadPlane::setup(void) } if (hal.util->available_memory() < - 4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav) + sizeof(*ahrs_view) + sizeof(*loiter_nav)) { + 4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav) + sizeof(*ahrs_view) + sizeof(*loiter_nav) + sizeof(*weathervane)) { AP_BoardConfig::config_error("Not enough memory for quadplane"); } @@ -692,6 +688,12 @@ bool QuadPlane::setup(void) } AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); + weathervane = new AC_WeatherVane(); + if (!weathervane) { + AP_BoardConfig::allocation_error("weathervane"); + } + AP_Param::load_object_from_eeprom(weathervane, weathervane->var_info); + motors->init(frame_class, frame_type); motors->update_throttle_range(); motors->set_update_rate(rc_speed); @@ -3254,52 +3256,32 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) { /* we only do weathervaning in modes where we are doing VTOL - position control. We also don't do it if the pilot has given any - yaw input in the last 3 seconds. + position control. */ if (!in_vtol_mode() || - !motors->armed() || - weathervane.gain <= 0 || + !motors->armed() || (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) || plane.control_mode == &plane.mode_qstabilize || #if QAUTOTUNE_ENABLED plane.control_mode == &plane.mode_qautotune || #endif - plane.control_mode == &plane.mode_qhover + plane.control_mode == &plane.mode_qhover || + should_relax() ) { - weathervane.last_output = 0; - return 0; - } - const uint32_t tnow = millis(); - if (plane.channel_rudder->get_control_in() != 0) { - weathervane.last_pilot_input_ms = tnow; - weathervane.last_output = 0; - return 0; - } - if (tnow - weathervane.last_pilot_input_ms < 3000) { - weathervane.last_output = 0; - return 0; + // Ensure the weathervane controller is reset to prevent weathervaning from happening outside of the timer + weathervane->reset(); + return 0.0; } - float roll = wp_nav->get_roll() / 100.0f; - if (fabsf(roll) < weathervane.min_roll) { - weathervane.last_output = 0; - return 0; + float wv_output; + if (weathervane->get_yaw_out(wv_output, + plane.channel_rudder->get_control_in(), + plane.relative_ground_altitude(plane.g.rangefinder_landing), + wp_nav->get_roll(), + wp_nav->get_pitch())) { + return constrain_float(wv_output / 45, -100.0, 100.0) * yaw_rate_max * 0.5; } - if (roll > 0) { - roll -= weathervane.min_roll; - } else { - roll += weathervane.min_roll; - } - - float output = constrain_float((roll/45.0f) * weathervane.gain, -1, 1); - if (should_relax()) { - output = 0; - } - weathervane.last_output = 0.98f * weathervane.last_output + 0.02f * output; - // scale over half of yaw_rate_max. This gives the pilot twice the - // authority of the weathervane controller - return weathervane.last_output * (yaw_rate_max/2) * 100; + return 0.0; } /* diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ad657af9ca..9a90c1de64 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -13,6 +13,7 @@ #include // Attitude control library #include #include +#include #include #include #include @@ -372,13 +373,8 @@ private: float last_pct; } vel_forward; - struct { - AP_Float gain; - AP_Float min_roll; - uint32_t last_pilot_input_ms; - float last_output; - } weathervane; - + AC_WeatherVane *weathervane; + bool initialised; Location last_auto_target;