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

Rover: add Acro mode 

ACRO_TURN_RATE allows user control of maximum turn rate
This commit is contained in:
Randy Mackay 2017-11-28 15:59:13 +09:00
parent 2c6593e35e
commit 04e9228fa0
8 changed files with 84 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
APMrover2/Parameters.cpp
View File

@ -41,7 +41,7 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: INITIAL_MODE
// @DisplayName: Initial driving mode
// @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART.
// @Values: 0:MANUAL,2:LEARNING,3:STEERING,4:HOLD,10:AUTO,11:RTL,15:GUIDED
// @Values: 0:MANUAL,1:ACRO,3:STEERING,4:HOLD,10:AUTO,11:RTL,15:GUIDED
// @User: Advanced
GSCALAR(initial_mode, "INITIAL_MODE", MANUAL),
@ -241,7 +241,7 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: MODE1
// @DisplayName: Mode1
// @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @User: Standard
// @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
GSCALAR(mode1, "MODE1", MANUAL),
@ -249,35 +249,35 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: MODE2
// @DisplayName: Mode2
// @Description: Driving mode for switch position 2 (1231 to 1360)
// @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode2, "MODE2", MANUAL),
// @Param: MODE3
// @DisplayName: Mode3
// @Description: Driving mode for switch position 3 (1361 to 1490)
// @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode3, "MODE3", MANUAL),
// @Param: MODE4
// @DisplayName: Mode4
// @Description: Driving mode for switch position 4 (1491 to 1620)
// @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode4, "MODE4", MANUAL),
// @Param: MODE5
// @DisplayName: Mode5
// @Description: Driving mode for switch position 5 (1621 to 1749)
// @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode5, "MODE5", MANUAL),
// @Param: MODE6
// @DisplayName: Mode6
// @Description: Driving mode for switch position 6 (1750 to 2049)
// @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @User: Standard
GSCALAR(mode6, "MODE6", MANUAL),
@ -504,6 +504,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard
AP_GROUPINFO("TURN_RADIUS", 11, ParametersG2, turn_radius, 0.9),
// @Param: ACRO_TURN_RATE
// @DisplayName: Acro mode turn rate maximum
// @Description: Acro mode turn rate maximum
// @Units: deg/s
// @Range: 0 360
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ACRO_TURN_RATE", 12, ParametersG2, acro_turn_rate, 180.0f),
AP_GROUPEND
};

3
APMrover2/Parameters.h
View File

@ -316,6 +316,9 @@ public:
// turn radius of vehicle (only used in steering mode)
AP_Float turn_radius;
// acro mode turn rate maximum
AP_Float acro_turn_rate;
};
extern const AP_Param::Info var_info[];

2
APMrover2/Rover.h
View File

@ -104,6 +104,7 @@ public:
#endif
friend class GCS_Rover;
friend class Mode;
friend class ModeAcro;
friend class ModeAuto;
friend class ModeGuided;
friend class ModeHold;
@ -378,6 +379,7 @@ private:
ModeInitializing mode_initializing;
ModeHold mode_hold;
ModeManual mode_manual;
ModeAcro mode_acro;
ModeGuided mode_guided;
ModeAuto mode_auto;
ModeSteering mode_steering;

7
APMrover2/control_modes.cpp
View File

@ -9,6 +9,9 @@ Mode *Rover::control_mode_from_num(const enum mode num)
case MANUAL:
ret = &mode_manual;
break;
case ACRO:
ret = &mode_acro;
break;
case STEERING:
ret = &mode_steering;
break;
@ -156,8 +159,8 @@ void Rover::read_aux_switch()
return;
}
// record the waypoint if in manual or steering modes
if (control_mode == &mode_manual || control_mode == &mode_steering) {
// record the waypoint if in manual, acro or steering mode
if (control_mode == &mode_manual || control_mode == &mode_acro ||control_mode == &mode_steering) {
// create new mission command
AP_Mission::Mission_Command cmd = {};

1
APMrover2/defines.h
View File

@ -28,6 +28,7 @@ enum ch7_option {
// ----------------
enum mode {
MANUAL = 0,
ACRO = 1,
STEERING = 3,
HOLD = 4,
AUTO = 10,

14
APMrover2/mode.h
View File

@ -145,6 +145,20 @@ protected:
};
class ModeAcro : public Mode
{
public:
uint32_t mode_number() const override { return ACRO; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes for mavlink system status reporting
bool has_manual_input() const override { return true; }
};
class ModeAuto : public Mode
{
public:

40
APMrover2/mode_acro.cpp Normal file
View File

@ -0,0 +1,40 @@
#include "mode.h"
#include "Rover.h"
void ModeAcro::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
return;
}
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// convert steering request to turn rate in radians/sec
float turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
// set reverse flag backing up
bool reversed = is_negative(target_speed);
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle();
} else {
// run steering turn rate controller and throttle controller
float steering_out = attitude_control.get_steering_out_rate(turn_rate, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(target_speed, false);
}
}

3
APMrover2/system.cpp
View File

@ -290,6 +290,9 @@ void Rover::notify_mode(enum mode new_mode)
case MANUAL:
notify.set_flight_mode_str("MANU");
break;
case ACRO:
notify.set_flight_mode_str("ACRO");
break;
case STEERING:
notify.set_flight_mode_str("STER");
break;