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

Rover: Vehicle will stop with loiter in Guided mode for boat.

This commit is contained in:
TsuyoshiKawamura 2018-12-04 04:25:52 +09:00 committed by Randy Mackay
parent 01d5ce33b2
commit a9781686b1
2 changed files with 36 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
APMrover2/mode.h
View File

@ -361,12 +361,16 @@ public:
void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
// vehicle start loiter
bool start_loiter();
protected:
enum GuidedMode {
Guided_WP,
Guided_HeadingAndSpeed,
Guided_TurnRateAndSpeed
Guided_TurnRateAndSpeed,
Guided_Loiter
};
bool _enter() override;

35
APMrover2/mode_guided.cpp
View File

@ -26,12 +26,16 @@ void ModeGuided::update()
rover.gcs().send_mission_item_reached_message(0);
}
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
if (!_reached_destination) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
} else {
stop_vehicle();
if (rover.is_boat() && !start_loiter()) {
stop_vehicle();
} else {
stop_vehicle();
}
}
break;
}
@ -48,7 +52,11 @@ void ModeGuided::update()
calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
} else {
stop_vehicle();
if (rover.is_boat() && !start_loiter()) {
stop_vehicle();
} else {
stop_vehicle();
}
}
break;
}
@ -69,11 +77,21 @@ void ModeGuided::update()
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true, true);
} else {
stop_vehicle();
if (rover.is_boat() && !start_loiter()) {
stop_vehicle();
} else {
stop_vehicle();
}
}
break;
}
case Guided_Loiter:
{
rover.mode_loiter.update();
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
@ -147,3 +165,12 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
// log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
}
bool ModeGuided::start_loiter()
{
if (rover.mode_loiter.enter()) {
_guided_mode = Guided_Loiter;
return true;
}
return false;
}