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.

Sign up for GitHub

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

Jump to bottom

ArduCopter: send MISSION_ITEM_REACHED on guided mode #3360

Closed
wants to merge 1 commit into from
Closed

ArduCopter: send MISSION_ITEM_REACHED on guided mode #3360

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
3 changes: 3 additions & 0 deletions ArduCopter/Copter.h
View file
Open in desktop
Expand Up @@ -339,6 +339,8 @@ class Copter : public AP_HAL::HAL::Callbacks {

// Guided
GuidedMode guided_mode; // controls which controller is run (pos or vel)
GuidedMode guided_prev_mode;
bool guided_reached_notified; // has guided reached destination been notified?

// RTL
RTLState rtl_state; // records state of rtl (initial climb, returning home, etc)
Expand Down Expand Up @@ -756,6 +758,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
void guided_run();
void guided_takeoff_run();
void guided_reached_destination_notify(bool check_altitude);
void guided_pos_control_run();
void guided_vel_control_run();
void guided_posvel_control_run();
Expand Down
29 changes: 29 additions & 0 deletions ArduCopter/control_guided.cpp
View file
Open in desktop
Expand Up @@ -39,6 +39,7 @@ struct Guided_Limit {
bool Copter::guided_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
guided_reached_notified = false;
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode
Expand Down Expand Up @@ -212,6 +213,30 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
guided_angle_state.update_time_ms = millis();
}

// guided_reached_destination_notify - notify if we've reached the destination
// on guided mode - if we're taking off we consider altitude
void Copter::guided_reached_destination_notify(bool check_altitude)
{
bool reached_alt = true;

if (guided_prev_mode != guided_mode)
guided_reached_notified = false;
Copy link
Contributor

Choose a reason for hiding this comment

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

What is the prev_mode for? Seems like there's got to be a better way to detect TO vs WP transitions than that. Also, it needs { } even for the one line.

Copy link
Author

Choose a reason for hiding this comment

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

prev_mode is indeed used to detect transitions between modes - currently mostly interested on takeoff and WP. What would be the better way to detect that?

Thanks...

Copy link
Contributor

Choose a reason for hiding this comment

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

When I say there's got to be a better way, see the Plane #3361 and Rover #3362 PRs I just made. Each were 2 line commits.

Copy link
Author

Choose a reason for hiding this comment

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

@magicrub as far as I could see the loiter mode on plane and rover doesn't work as the guided mode on copters, the loiter start_time_ms is always reset when a new nav command is sent to APM, this doesn't happen to guided for copters.

The code you're changing in the mentioned commits use that condition to identify a mode has changed, since we got a new nav command the start_time_ms is set to zero and we can assume we're transitioning between modes or maybe we're "about to process a new command", so we're ok to send the item reached message based on that assumption, but we don't have the very same behavior for copters.

Am I maybe missing something here?

Thanks...


if (check_altitude) {
const Vector3f& curr_pos = inertial_nav.get_position();
const Vector3f& target_pos = wp_nav.get_wp_destination();

if (curr_pos.z < target_pos.z) {
reached_alt = false;
}
}

if (reached_alt && wp_nav.reached_wp_destination() && !guided_reached_notified) {
gcs_send_mission_item_reached_message(0);
guided_reached_notified = true;
}
}

// guided_run - runs the guided controller
// should be called at 100hz or more
void Copter::guided_run()
Expand All @@ -222,11 +247,13 @@ void Copter::guided_run()
case Guided_TakeOff:
// run takeoff controller
guided_takeoff_run();
guided_reached_destination_notify(true);
break;

case Guided_WP:
// run position controller
guided_pos_control_run();
guided_reached_destination_notify(false);
break;

case Guided_Velocity:
Expand All @@ -244,6 +271,8 @@ void Copter::guided_run()
guided_angle_control_run();
break;
}

guided_prev_mode = guided_mode;
}

// guided_takeoff_run - takeoff in guided mode
Expand Down