aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use new weathervane library

Browse Source
This commit is contained in:
Gone4Dirt 2022-01-23 22:21:18 +00:00 committed by Andrew Tridgell
parent cc545efa29
commit a79359dc67
3 changed files with 53 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduPlane/RC_Channel.cpp
View File

@ -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:

82
ArduPlane/quadplane.cpp
View File

@ -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;
}
/*

10
ArduPlane/quadplane.h
View File

@ -13,6 +13,7 @@
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
#include <AP_InertialNav/AP_InertialNav.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AC_AttitudeControl/AC_WeatherVane.h>
#include <AC_WPNav/AC_WPNav.h>
#include <AC_WPNav/AC_Loiter.h>
#include <AC_Fence/AC_Fence.h>
@ -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;