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: Adding support for rovers driving in Reverse.

Browse Source
This commit is contained in:
Grant Morphett 2016-07-14 17:50:39 +10:00 committed by Andrew Tridgell
parent 8d6b417adb
commit 57c23c7fe3
5 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
APMrover2/APMrover2.cpp
View File

@ -411,14 +411,12 @@ void Rover::update_current_mode(void)
switch (control_mode){ switch (control_mode){
case AUTO: case AUTO:
case RTL: case RTL:
set_reverse(false);
calc_lateral_acceleration(); calc_lateral_acceleration();
calc_nav_steer(); calc_nav_steer();
calc_throttle(g.speed_cruise); calc_throttle(g.speed_cruise);
break; break;
case GUIDED: case GUIDED:
set_reverse(false);
if (rtl_complete || verify_RTL()) { if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are // we have reached destination so stop where we are
if (channel_throttle->get_servo_out() != g.throttle_min.get()) { if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
@ -483,7 +481,6 @@ void Rover::update_current_mode(void)
// hold position - stop motors and center steering // hold position - stop motors and center steering
channel_throttle->set_servo_out(0); channel_throttle->set_servo_out(0);
channel_steer->set_servo_out(0); channel_steer->set_servo_out(0);
set_reverse(false);
break; break;
case INITIALISING: case INITIALISING:

1
APMrover2/Rover.h
View File

@ -511,6 +511,7 @@ private:
void do_set_home(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd);
void do_digicam_configure(const AP_Mission::Mission_Command& cmd); void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
void do_digicam_control(const AP_Mission::Mission_Command& cmd); void do_digicam_control(const AP_Mission::Mission_Command& cmd);
void do_set_reverse(const AP_Mission::Mission_Command& cmd);
void init_capabilities(void); void init_capabilities(void);
void rudder_arm_disarm_check(); void rudder_arm_disarm_check();
void change_arm_state(void); void change_arm_state(void);

4
APMrover2/Steering.cpp
View File

@ -203,7 +203,9 @@ void Rover::calc_nav_steer() {
} }
// add in obstacle avoidance // add in obstacle avoidance
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g; if (!in_reverse) {
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
}
// constrain to max G force // constrain to max G force
lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS); lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);

13
APMrover2/commands_logic.cpp
View File

@ -107,6 +107,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
break; break;
#endif #endif
case MAV_CMD_DO_SET_REVERSE:
do_set_reverse(cmd);
break;
default: default:
// return false for unhandled commands // return false for unhandled commands
return false; return false;
@ -405,3 +409,12 @@ void Rover::log_picture()
} }
} }
} }
void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1) {
set_reverse(true);
} else {
set_reverse(false);
}
}

4
APMrover2/system.cpp
View File

@ -250,6 +250,9 @@ void Rover::set_reverse(bool reverse)
return; return;
} }
g.pidSpeedThrottle.reset_I(); g.pidSpeedThrottle.reset_I();
steerController.reset_I();
nav_controller->set_reverse(reverse);
steerController.set_reverse(reverse);
in_reverse = reverse; in_reverse = reverse;
} }
@ -269,7 +272,6 @@ void Rover::set_mode(enum mode mode)
control_mode = mode; control_mode = mode;
throttle_last = 0; throttle_last = 0;
throttle = 500; throttle = 500;
set_reverse(false);
g.pidSpeedThrottle.reset_I(); g.pidSpeedThrottle.reset_I();
if (control_mode != AUTO) { if (control_mode != AUTO) {