mirror of https://github.com/ArduPilot/ardupilot
Rover: add Acro mode
ACRO_TURN_RATE allows user control of maximum turn rate
This commit is contained in:
parent
2c6593e35e
commit
04e9228fa0
|
@ -41,7 +41,7 @@ const AP_Param::Info Rover::var_info[] = {
|
||||||
// @Param: INITIAL_MODE
|
// @Param: INITIAL_MODE
|
||||||
// @DisplayName: Initial driving 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.
|
// @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
|
// @User: Advanced
|
||||||
GSCALAR(initial_mode, "INITIAL_MODE", MANUAL),
|
GSCALAR(initial_mode, "INITIAL_MODE", MANUAL),
|
||||||
|
|
||||||
|
@ -241,7 +241,7 @@ const AP_Param::Info Rover::var_info[] = {
|
||||||
|
|
||||||
// @Param: MODE1
|
// @Param: MODE1
|
||||||
// @DisplayName: 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
|
// @User: Standard
|
||||||
// @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
|
// @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
|
||||||
GSCALAR(mode1, "MODE1", MANUAL),
|
GSCALAR(mode1, "MODE1", MANUAL),
|
||||||
|
@ -249,35 +249,35 @@ const AP_Param::Info Rover::var_info[] = {
|
||||||
// @Param: MODE2
|
// @Param: MODE2
|
||||||
// @DisplayName: Mode2
|
// @DisplayName: Mode2
|
||||||
// @Description: Driving mode for switch position 2 (1231 to 1360)
|
// @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
|
// @User: Standard
|
||||||
GSCALAR(mode2, "MODE2", MANUAL),
|
GSCALAR(mode2, "MODE2", MANUAL),
|
||||||
|
|
||||||
// @Param: MODE3
|
// @Param: MODE3
|
||||||
// @DisplayName: Mode3
|
// @DisplayName: Mode3
|
||||||
// @Description: Driving mode for switch position 3 (1361 to 1490)
|
// @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
|
// @User: Standard
|
||||||
GSCALAR(mode3, "MODE3", MANUAL),
|
GSCALAR(mode3, "MODE3", MANUAL),
|
||||||
|
|
||||||
// @Param: MODE4
|
// @Param: MODE4
|
||||||
// @DisplayName: Mode4
|
// @DisplayName: Mode4
|
||||||
// @Description: Driving mode for switch position 4 (1491 to 1620)
|
// @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
|
// @User: Standard
|
||||||
GSCALAR(mode4, "MODE4", MANUAL),
|
GSCALAR(mode4, "MODE4", MANUAL),
|
||||||
|
|
||||||
// @Param: MODE5
|
// @Param: MODE5
|
||||||
// @DisplayName: Mode5
|
// @DisplayName: Mode5
|
||||||
// @Description: Driving mode for switch position 5 (1621 to 1749)
|
// @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
|
// @User: Standard
|
||||||
GSCALAR(mode5, "MODE5", MANUAL),
|
GSCALAR(mode5, "MODE5", MANUAL),
|
||||||
|
|
||||||
// @Param: MODE6
|
// @Param: MODE6
|
||||||
// @DisplayName: Mode6
|
// @DisplayName: Mode6
|
||||||
// @Description: Driving mode for switch position 6 (1750 to 2049)
|
// @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
|
// @User: Standard
|
||||||
GSCALAR(mode6, "MODE6", MANUAL),
|
GSCALAR(mode6, "MODE6", MANUAL),
|
||||||
|
|
||||||
|
@ -504,6 +504,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO("TURN_RADIUS", 11, ParametersG2, turn_radius, 0.9),
|
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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -316,6 +316,9 @@ public:
|
||||||
|
|
||||||
// turn radius of vehicle (only used in steering mode)
|
// turn radius of vehicle (only used in steering mode)
|
||||||
AP_Float turn_radius;
|
AP_Float turn_radius;
|
||||||
|
|
||||||
|
// acro mode turn rate maximum
|
||||||
|
AP_Float acro_turn_rate;
|
||||||
};
|
};
|
||||||
|
|
||||||
extern const AP_Param::Info var_info[];
|
extern const AP_Param::Info var_info[];
|
||||||
|
|
|
@ -104,6 +104,7 @@ public:
|
||||||
#endif
|
#endif
|
||||||
friend class GCS_Rover;
|
friend class GCS_Rover;
|
||||||
friend class Mode;
|
friend class Mode;
|
||||||
|
friend class ModeAcro;
|
||||||
friend class ModeAuto;
|
friend class ModeAuto;
|
||||||
friend class ModeGuided;
|
friend class ModeGuided;
|
||||||
friend class ModeHold;
|
friend class ModeHold;
|
||||||
|
@ -378,6 +379,7 @@ private:
|
||||||
ModeInitializing mode_initializing;
|
ModeInitializing mode_initializing;
|
||||||
ModeHold mode_hold;
|
ModeHold mode_hold;
|
||||||
ModeManual mode_manual;
|
ModeManual mode_manual;
|
||||||
|
ModeAcro mode_acro;
|
||||||
ModeGuided mode_guided;
|
ModeGuided mode_guided;
|
||||||
ModeAuto mode_auto;
|
ModeAuto mode_auto;
|
||||||
ModeSteering mode_steering;
|
ModeSteering mode_steering;
|
||||||
|
|
|
@ -9,6 +9,9 @@ Mode *Rover::control_mode_from_num(const enum mode num)
|
||||||
case MANUAL:
|
case MANUAL:
|
||||||
ret = &mode_manual;
|
ret = &mode_manual;
|
||||||
break;
|
break;
|
||||||
|
case ACRO:
|
||||||
|
ret = &mode_acro;
|
||||||
|
break;
|
||||||
case STEERING:
|
case STEERING:
|
||||||
ret = &mode_steering;
|
ret = &mode_steering;
|
||||||
break;
|
break;
|
||||||
|
@ -156,8 +159,8 @@ void Rover::read_aux_switch()
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// record the waypoint if in manual or steering modes
|
// record the waypoint if in manual, acro or steering mode
|
||||||
if (control_mode == &mode_manual || control_mode == &mode_steering) {
|
if (control_mode == &mode_manual || control_mode == &mode_acro ||control_mode == &mode_steering) {
|
||||||
// create new mission command
|
// create new mission command
|
||||||
AP_Mission::Mission_Command cmd = {};
|
AP_Mission::Mission_Command cmd = {};
|
||||||
|
|
||||||
|
|
|
@ -28,6 +28,7 @@ enum ch7_option {
|
||||||
// ----------------
|
// ----------------
|
||||||
enum mode {
|
enum mode {
|
||||||
MANUAL = 0,
|
MANUAL = 0,
|
||||||
|
ACRO = 1,
|
||||||
STEERING = 3,
|
STEERING = 3,
|
||||||
HOLD = 4,
|
HOLD = 4,
|
||||||
AUTO = 10,
|
AUTO = 10,
|
||||||
|
|
|
@ -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
|
class ModeAuto : public Mode
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
|
@ -290,6 +290,9 @@ void Rover::notify_mode(enum mode new_mode)
|
||||||
case MANUAL:
|
case MANUAL:
|
||||||
notify.set_flight_mode_str("MANU");
|
notify.set_flight_mode_str("MANU");
|
||||||
break;
|
break;
|
||||||
|
case ACRO:
|
||||||
|
notify.set_flight_mode_str("ACRO");
|
||||||
|
break;
|
||||||
case STEERING:
|
case STEERING:
|
||||||
notify.set_flight_mode_str("STER");
|
notify.set_flight_mode_str("STER");
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue