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

Copter: ModeGuided: add an option_is_enabled method, use it

Browse Source
This commit is contained in:
Peter Barker 2024-05-23 20:25:30 +10:00 committed by Peter Barker
parent d305cb47f8
commit 1c5a026e3e
2 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/mode.h
View File

@ -1127,7 +1127,7 @@ protected:
private: private:
// enum for GUID_OPTIONS parameter // enum for GUID_OPTIONS parameter
enum class Options : int32_t { enum class Option : uint32_t {
AllowArmingFromTX = (1U << 0), AllowArmingFromTX = (1U << 0),
// this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto // this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto
IgnorePilotYaw = (1U << 2), IgnorePilotYaw = (1U << 2),
@ -1138,6 +1138,9 @@ private:
AllowWeatherVaning = (1U << 7) AllowWeatherVaning = (1U << 7)
}; };
// returns true if the Guided-mode-option is set (see GUID_OPTIONS)
bool option_is_enabled(Option option) const;
// wp controller // wp controller
void wp_control_start(); void wp_control_start();
void wp_control_run(); void wp_control_run();

20
ArduCopter/mode_guided.cpp
View File

@ -97,6 +97,12 @@ void ModeGuided::run()
} }
} }
// returns true if the Guided-mode-option is set (see GUID_OPTIONS)
bool ModeGuided::option_is_enabled(Option option) const
{
return (copter.g2.guided_options.get() & (uint32_t)option) != 0;
}
bool ModeGuided::allows_arming(AP_Arming::Method method) const bool ModeGuided::allows_arming(AP_Arming::Method method) const
{ {
// always allow arming from the ground station or scripting // always allow arming from the ground station or scripting
@ -105,13 +111,13 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
} }
// optionally allow arming from the transmitter // optionally allow arming from the transmitter
return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0; return option_is_enabled(Option::AllowArmingFromTX);
}; };
#if WEATHERVANE_ENABLED == ENABLED #if WEATHERVANE_ENABLED == ENABLED
bool ModeGuided::allows_weathervaning() const bool ModeGuided::allows_weathervaning() const
{ {
return (copter.g2.guided_options.get() & (uint32_t)Options::AllowWeatherVaning) != 0; return option_is_enabled(Option::AllowWeatherVaning);
} }
#endif #endif
@ -607,25 +613,25 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate // returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
bool ModeGuided::set_attitude_target_provides_thrust() const bool ModeGuided::set_attitude_target_provides_thrust() const
{ {
return ((copter.g2.guided_options.get() & uint32_t(Options::SetAttitudeTarget_ThrustAsThrust)) != 0); return option_is_enabled(Option::SetAttitudeTarget_ThrustAsThrust);
} }
// returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active) // returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active)
bool ModeGuided::stabilizing_pos_xy() const bool ModeGuided::stabilizing_pos_xy() const
{ {
return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizePositionXY)) != 0); return !option_is_enabled(Option::DoNotStabilizePositionXY);
} }
// returns true if GUIDED_OPTIONS param specifies velocity should be controlled (when acceleration control is active) // returns true if GUIDED_OPTIONS param specifies velocity should be controlled (when acceleration control is active)
bool ModeGuided::stabilizing_vel_xy() const bool ModeGuided::stabilizing_vel_xy() const
{ {
return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0); return !option_is_enabled(Option::DoNotStabilizeVelocityXY);
} }
// returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower) // returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
bool ModeGuided::use_wpnav_for_position_control() const bool ModeGuided::use_wpnav_for_position_control() const
{ {
return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0); return option_is_enabled(Option::WPNavUsedForPosControl);
} }
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option) // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
@ -1009,7 +1015,7 @@ void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, fl
// returns true if pilot's yaw input should be used to adjust vehicle's heading // returns true if pilot's yaw input should be used to adjust vehicle's heading
bool ModeGuided::use_pilot_yaw(void) const bool ModeGuided::use_pilot_yaw(void) const
{ {
return (copter.g2.guided_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0; return !option_is_enabled(Option::IgnorePilotYaw);
} }
// Guided Limit code // Guided Limit code