Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: add yaw weathevaneing mode

This commit is contained in:
Gone4Dirt 2022-04-14 10:58:27 +01:00 committed by Bill Geyer
parent 650f0ff659
commit 66a4ba6256
7 changed files with 104 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/Parameters.cpp
View File

@ -983,7 +983,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: AUTO_OPTIONS
// @DisplayName: Auto mode options
// @Description: A range of options that can be applied to change auto mode behaviour. Allow Arming allows the copter to be armed in Auto. Allow Takeoff Without Raising Throttle allows takeoff without the pilot having to raise the throttle. Ignore pilot yaw overrides the pilot's yaw stick being used while in auto.
// @Bitmask: 0:Allow Arming,1:Allow Takeoff Without Raising Throttle,2:Ignore pilot yaw
// @Bitmask: 0:Allow Arming,1:Allow Takeoff Without Raising Throttle,2:Ignore pilot yaw,7:Allow weathervaning
// @User: Advanced
AP_GROUPINFO("AUTO_OPTIONS", 40, ParametersG2, auto_options, 0),
#endif
@ -992,7 +992,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: GUID_OPTIONS
// @DisplayName: Guided mode options
// @Description: Options that can be applied to change guided mode behaviour
// @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY,6:Waypoint navigation used for position targets
// @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY,6:Waypoint navigation used for position targets,7:Allow weathervaning
// @User: Advanced
AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0),
#endif
@ -1160,6 +1160,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("TKOFF_RPM_MIN", 58, ParametersG2, takeoff_rpm_min, 0),
#endif
#if WEATHERVANE_ENABLED == ENABLED
// @Group: WVANE_
// @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp
AP_SUBGROUPINFO(weathervane, "WVANE_", 59, ParametersG2, AC_WeatherVane),
#endif
// ID 60 is reserved for the SHIP_OPS
// ID 62 is reserved for the SHOW_... parameters from the Skybrush fork at
@ -1219,6 +1225,10 @@ ParametersG2::ParametersG2(void)
#endif
,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f)
#if WEATHERVANE_ENABLED == ENABLED
,weathervane()
#endif
{
AP_Param::setup_object_defaults(this, var_info);
}

7
ArduCopter/Parameters.h
View File

@ -11,6 +11,9 @@
#if MODE_FOLLOW_ENABLED == ENABLED
# include <AP_Follow/AP_Follow.h>
#endif
#if WEATHERVANE_ENABLED == ENABLED
#include <AC_AttitudeControl/AC_WeatherVane.h>
#endif
// Global parameter class.
//
@ -674,6 +677,10 @@ public:
#if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME
AP_Int16 takeoff_rpm_min;
#endif
#if WEATHERVANE_ENABLED == ENABLED
AC_WeatherVane weathervane;
#endif
};
extern const AP_Param::Info var_info[];

40
ArduCopter/autoyaw.cpp
View File

@ -56,6 +56,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
if (_mode == yaw_mode) {
return;
}
_last_mode = _mode;
_mode = yaw_mode;
// perform initialisation
@ -96,6 +97,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
case Mode::CIRCLE:
case Mode::PILOT_RATE:
case Mode::WEATHERVANE:
// no initialisation required
break;
}
@ -279,6 +281,7 @@ float Mode::AutoYaw::rate_cds() const
case Mode::ANGLE_RATE:
case Mode::RATE:
case Mode::WEATHERVANE:
return _yaw_rate_cds;
case Mode::LOOK_AT_NEXT_WP:
@ -307,6 +310,10 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
#if WEATHERVANE_ENABLED == ENABLED
update_weathervane(_pilot_yaw_rate_cds);
#endif
AC_AttitudeControl::HeadingCommand heading;
heading.yaw_angle_cd = yaw();
heading.yaw_rate_cds = auto_yaw.rate_cds();
@ -315,6 +322,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
case Mode::HOLD:
case Mode::RATE:
case Mode::PILOT_RATE:
case Mode::WEATHERVANE:
heading.heading_mode = AC_AttitudeControl::HeadingMode::Rate_Only;
break;
case Mode::LOOK_AT_NEXT_WP:
@ -330,3 +338,35 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
return heading;
}
// handle the interface to the weathervane library
// pilot_yaw can be an angle or a rate or rcin from yaw channel. It just needs to represent a pilot's request to yaw the vehicle to enable pilot overrides.
#if WEATHERVANE_ENABLED == ENABLED
void Mode::AutoYaw::update_weathervane(const int16_t pilot_yaw_cds)
{
if (!copter.flightmode->allows_weathervaning()) {
return;
}
float yaw_rate_cds;
if (copter.g2.weathervane.get_yaw_out(yaw_rate_cds, pilot_yaw_cds, copter.flightmode->get_alt_above_ground_cm()*0.01,
copter.pos_control->get_roll_cd(),
copter.pos_control->get_pitch_cd(),
copter.flightmode->is_taking_off(),
copter.flightmode->is_landing())) {
set_mode(Mode::WEATHERVANE);
_yaw_rate_cds = yaw_rate_cds;
return;
}
// if the weathervane controller has previously been activated we need to ensure we return control back to what was previously set
if (mode() == Mode::WEATHERVANE) {
_yaw_rate_cds = 0.0;
if (_last_mode == Mode::HOLD) {
set_mode_to_default(false);
} else {
set_mode(_last_mode);
}
}
}
#endif // WEATHERVANE_ENABLED == ENABLED

8
ArduCopter/config.h
View File

@ -278,6 +278,14 @@
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Weathervane - allow vehicle to yaw into wind
#ifndef WEATHERVANE_ENABLED
# define WEATHERVANE_ENABLED !HAL_MINIMIZE_FEATURES
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Autorotate - autonomous auto-rotation - helicopters only
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

23
ArduCopter/mode.h
View File

@ -122,6 +122,11 @@ public:
virtual bool pause() { return false; };
virtual bool resume() { return false; };
// true if weathervaning is allowed in the current mode
#if WEATHERVANE_ENABLED == ENABLED
virtual bool allows_weathervaning() const { return false; }
#endif
protected:
// helper functions
@ -244,6 +249,7 @@ public:
RATE = 7, // turn at a specified rate (held in auto_yaw_rate)
CIRCLE = 8, // use AC_Circle's provided yaw (used during Loiter-Turns commands)
PILOT_RATE = 9, // target rate from pilot stick
WEATHERVANE = 10, // yaw into wind
};
// mode(): current method of determining desired yaw:
@ -267,6 +273,10 @@ public:
bool reached_fixed_yaw_target();
#if WEATHERVANE_ENABLED == ENABLED
void update_weathervane(const int16_t pilot_yaw_cds);
#endif
AC_AttitudeControl::HeadingCommand get_heading();
private:
@ -282,6 +292,7 @@ public:
// auto flight mode's yaw mode
Mode _mode = Mode::LOOK_AT_NEXT_WP;
Mode _last_mode;
// Yaw will point at this location if mode is set to Mode::ROI
Vector3f roi;
@ -498,6 +509,11 @@ public:
// Mission change detector
AP_Mission_ChangeDetector mis_change_detector;
// true if weathervaning is allowed in auto
#if WEATHERVANE_ENABLED == ENABLED
bool allows_weathervaning(void) const override;
#endif
protected:
const char *name() const override { return auto_RTL? "AUTO RTL" : "AUTO"; }
@ -514,6 +530,7 @@ private:
AllowArming = (1 << 0U),
AllowTakeOffWithoutRaisingThrottle = (1 << 1U),
IgnorePilotYaw = (1 << 2U),
AllowWeatherVaning = (1 << 7U),
};
bool start_command(const AP_Mission::Mission_Command& cmd);
@ -1030,6 +1047,11 @@ public:
bool pause() override;
bool resume() override;
// true if weathervaning is allowed in guided
#if WEATHERVANE_ENABLED == ENABLED
bool allows_weathervaning(void) const override;
#endif
protected:
const char *name() const override { return "GUIDED"; }
@ -1050,6 +1072,7 @@ private:
DoNotStabilizePositionXY = (1U << 4),
DoNotStabilizeVelocityXY = (1U << 5),
WPNavUsedForPosControl = (1U << 6),
AllowWeatherVaning = (1U << 7)
};
// wp controller

7
ArduCopter/mode_auto.cpp
View File

@ -199,6 +199,13 @@ bool ModeAuto::allows_arming(AP_Arming::Method method) const
return ((copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0) && !auto_RTL;
};
#if WEATHERVANE_ENABLED == ENABLED
bool ModeAuto::allows_weathervaning() const
{
return (copter.g2.auto_options & (uint32_t)Options::AllowWeatherVaning) != 0;
}
#endif
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
{

7
ArduCopter/mode_guided.cpp
View File

@ -108,6 +108,13 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0;
};
#if WEATHERVANE_ENABLED == ENABLED
bool ModeGuided::allows_weathervaning() const
{
return (copter.g2.guided_options.get() & (uint32_t)Options::AllowWeatherVaning) != 0;
}
#endif
// initialises position controller to implement take-off
// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)