Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Create and use enumerations for ThrowMode #15982

Merged
merged 2 commits into from
Dec 8, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -699,7 +699,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Description: Used by Throw mode. Controls whether motors will run at the speed set by MOT_SPIN_MIN or will be stopped when armed and waiting for the throw.
// @Values: 0:Stopped,1:Running
// @User: Standard
GSCALAR(throw_motor_start, "THROW_MOT_START", 0),
GSCALAR(throw_motor_start, "THROW_MOT_START", (float)ModeThrow::PreThrowMotorState::STOPPED),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i'm surprised we need the float cast

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[717/746] Compiling ArduCopter/Parameters.cpp
../../ArduCopter/Parameters.cpp:727:1: error: cannot convert 'ModeThrow::PreThrowMotorState' to 'const float' in initialization
 };
 ^
compilation terminated due to -Wfatal-errors.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

lets move the cast to GSCALAR macro

#endif

// @Param: RTL_ALT_TYPE
Expand Down Expand Up @@ -757,7 +757,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Description: Used by Throw mode. Specifies whether Copter is thrown upward or dropped.
// @Values: 0:Upward Throw,1:Drop
// @User: Standard
AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ModeThrow::ThrowType_Upward),
AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, (float)ModeThrow::ThrowType::Upward),
#endif

// @Param: GND_EFFECT_COMP
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -447,7 +447,7 @@ class Parameters {
AP_Int16 gcs_pid_mask;

#if MODE_THROW_ENABLED == ENABLED
AP_Int8 throw_motor_start;
AP_Enum<ModeThrow::PreThrowMotorState> throw_motor_start;
#endif

AP_Int8 rtl_alt_type;
Expand Down Expand Up @@ -497,7 +497,7 @@ class ParametersG2 {
#if MODE_THROW_ENABLED == ENABLED
// Throw mode parameters
AP_Int8 throw_nextmode;
AP_Int8 throw_type;
AP_Enum<ModeThrow::ThrowType> throw_type;
#endif

// ground effect compensation enable/disable
Expand Down
11 changes: 8 additions & 3 deletions ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -1335,9 +1335,14 @@ class ModeThrow : public Mode {
bool is_autopilot() const override { return false; }

// Throw types
enum ThrowModeType {
ThrowType_Upward = 0,
ThrowType_Drop = 1
enum class ThrowType {
Upward = 0,
Drop = 1
};

enum class PreThrowMotorState {
STOPPED = 0,
RUNNING = 1,
};

protected:
Expand Down
8 changes: 4 additions & 4 deletions ArduCopter/mode_throw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ void ModeThrow::run()

// initialise the demanded height to 3m above the throw height
// we want to rapidly clear surrounding obstacles
if (g2.throw_type == ThrowType_Drop) {
if (g2.throw_type == ThrowType::Drop) {
pos_control->set_alt_target(inertial_nav.get_altitude() - 100);
} else {
pos_control->set_alt_target(inertial_nav.get_altitude() + 300);
Expand Down Expand Up @@ -108,7 +108,7 @@ void ModeThrow::run()
case Throw_Disarmed:

// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
Expand All @@ -123,7 +123,7 @@ void ModeThrow::run()
case Throw_Detecting:

// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
Expand Down Expand Up @@ -244,7 +244,7 @@ bool ModeThrow::throw_detected()

// check for upwards or downwards trajectory (airdrop) of 50cm/s
bool changing_height;
if (g2.throw_type == ThrowType_Drop) {
if (g2.throw_type == ThrowType::Drop) {
changing_height = inertial_nav.get_velocity().z < -THROW_VERTICAL_SPEED;
} else {
changing_height = inertial_nav.get_velocity().z > THROW_VERTICAL_SPEED;
Expand Down