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

Rover: support DO_SET_REVERSE commands in guided, RTL, SmartRTL

This commit is contained in:
Raouf 2018-08-08 14:18:13 +09:00 committed by Randy Mackay
parent 80753430e8
commit df3b6202f5
6 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/GCS_Mavlink.cpp
View File

@ -537,6 +537,14 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_SET_REVERSE:
// param1 : Direction (0=Forward, 1=Reverse)
if (is_equal(packet.param1, 1.0f)) {
rover.control_mode->set_reversed(1);
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
case MAV_CMD_DO_SET_HOME: {
// assume failure
if (is_equal(packet.param1, 1.0f)) {
@ -693,13 +701,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_SET_REVERSE:
// param1 : Direction (0=Forward, 1=Reverse)
if(!rover.control_mode->set_reversed(packet.param1)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_SET_HOME:
{
// param1 : use current (1=use current location, 0=use specified location)

2
APMrover2/commands_logic.cpp
View File

@ -418,5 +418,5 @@ void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd)
void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
{
mode_auto.set_reversed(cmd.p1 == 1);
rover.control_mode->set_reversed(cmd.p1 == 1);
}

1
APMrover2/mode.h
View File

@ -251,7 +251,6 @@ public:
// start RTL (within auto)
void start_RTL();
protected:
bool _enter() override;

4
APMrover2/mode_guided.cpp
View File

@ -29,8 +29,8 @@ void ModeGuided::update()
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
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();
}

4
APMrover2/mode_rtl.cpp
View File

@ -31,8 +31,8 @@ void ModeRTL::update()
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
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, false);
} else {
// we've reached destination so stop
stop_vehicle();

8
APMrover2/mode_smart_rtl.cpp
View File

@ -64,8 +64,8 @@ void ModeSmartRTL::update()
_load_point = true;
}
// continue driving towards destination
calc_steering_to_waypoint(_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
calc_steering_to_waypoint(_origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
break;
case SmartRTL_StopAtHome:
@ -73,8 +73,8 @@ void ModeSmartRTL::update()
_reached_destination = true;
if (rover.is_boat()) {
// boats attempt to hold position at home
calc_steering_to_waypoint(rover.current_loc, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
calc_steering_to_waypoint(rover.current_loc, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
} else {
// rovers stop
stop_vehicle();