Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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_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); void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
// vehicle start loiter
bool start_loiter();
protected: protected:
enum GuidedMode { enum GuidedMode {
Guided_WP, Guided_WP,
Guided_HeadingAndSpeed, Guided_HeadingAndSpeed,
Guided_TurnRateAndSpeed Guided_TurnRateAndSpeed,
Guided_Loiter
}; };
bool _enter() override; bool _enter() override;

29
APMrover2/mode_guided.cpp
View File

@ -26,12 +26,16 @@ void ModeGuided::update()
rover.gcs().send_mission_item_reached_message(0); rover.gcs().send_mission_item_reached_message(0);
} }
// determine if we should keep navigating // determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) { if (!_reached_destination) {
// drive towards destination // drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed); 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); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
} else { } else {
if (rover.is_boat() && !start_loiter()) {
stop_vehicle(); stop_vehicle();
} else {
stop_vehicle();
}
} }
break; break;
} }
@ -48,7 +52,11 @@ void ModeGuided::update()
calc_steering_to_heading(_desired_yaw_cd); calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
} else { } else {
if (rover.is_boat() && !start_loiter()) {
stop_vehicle(); stop_vehicle();
} else {
stop_vehicle();
}
} }
break; break;
} }
@ -68,9 +76,19 @@ void ModeGuided::update()
rover.G_Dt); rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f); g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true, true); calc_throttle(_desired_speed, true, true);
} else {
if (rover.is_boat() && !start_loiter()) {
stop_vehicle();
} else { } else {
stop_vehicle(); stop_vehicle();
} }
}
break;
}
case Guided_Loiter:
{
rover.mode_loiter.update();
break; break;
} }
@ -147,3 +165,12 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
// log new target // 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)); 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;
}