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

Rover: Updates to guided mode to work correctly on Rover

Browse Source
This commit is contained in:
Grant Morphett 2015-05-08 10:50:16 +10:00 committed by Andrew Tridgell
parent 2552acbf12
commit 0f13bc6d2a
4 changed files with 39 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
APMrover2/APMrover2.pde
View File

@ -805,13 +805,28 @@ static void update_current_mode(void)
switch (control_mode){
case AUTO:
case RTL:
case GUIDED:
set_reverse(false);
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
break;
case GUIDED:
set_reverse(false);
if (!rtl_complete) {
if (verify_RTL()) {
// we have reached destination so stop where we are
channel_throttle->servo_out = g.throttle_min.get();
channel_steer->servo_out = 0;
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
}
}
break;
case STEERING: {
/*
in steering mode we control lateral acceleration
@ -884,7 +899,6 @@ static void update_navigation()
break;
case RTL:
case GUIDED:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
@ -893,6 +907,20 @@ static void update_navigation()
set_mode(HOLD);
}
break;
case GUIDED:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (!rtl_complete) {
if (verify_RTL()) {
// we have reached destination so stop where we are
channel_throttle->servo_out = g.throttle_min.get();
channel_steer->servo_out = 0;
lateral_acceleration = 0;
}
}
break;
}
}

1
APMrover2/GCS_Mavlink.pde
View File

@ -807,6 +807,7 @@ void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
rtl_complete = false;
set_guided_WP();
}

5
APMrover2/commands_logic.pde
View File

@ -216,15 +216,16 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
static bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached Destination"));
rtl_complete = true;
return true;
}
// have we gone past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_fmt(PSTR("Reached Home dist %um"),
gcs_send_text_fmt(PSTR("Reached Destination: Distance away %um"),
(unsigned)get_distance(current_loc, next_WP));
rtl_complete = true;
return true;
}

5
APMrover2/system.pde
View File

@ -301,6 +301,11 @@ static void set_mode(enum mode mode)
do_RTL();
break;
case GUIDED:
rtl_complete = false;
set_guided_WP();
break;
default:
do_RTL();
break;