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

Rover: support DO_SET_REVERSE commands in guided, RTL, SmartRTL

Browse Source
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; 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: { case MAV_CMD_DO_SET_HOME: {
// assume failure // assume failure
if (is_equal(packet.param1, 1.0f)) { 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; 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: case MAV_CMD_DO_SET_HOME:
{ {
// param1 : use current (1=use current location, 0=use specified location) // 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) 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) // start RTL (within auto)
void start_RTL(); void start_RTL();
protected: protected:
bool _enter() override; bool _enter() override;

4
APMrover2/mode_guided.cpp
View File

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

4
APMrover2/mode_rtl.cpp
View File

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

8
APMrover2/mode_smart_rtl.cpp
View File

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