From 04e9228fa073c10ebe6f6f1f83f9d3c29a974315 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Nov 2017 15:59:13 +0900 Subject: [PATCH] Rover: add Acro mode ACRO_TURN_RATE allows user control of maximum turn rate --- APMrover2/Parameters.cpp | 23 ++++++++++++++------- APMrover2/Parameters.h | 3 +++ APMrover2/Rover.h | 2 ++ APMrover2/control_modes.cpp | 7 +++++-- APMrover2/defines.h | 1 + APMrover2/mode.h | 14 +++++++++++++ APMrover2/mode_acro.cpp | 40 +++++++++++++++++++++++++++++++++++++ APMrover2/system.cpp | 3 +++ 8 files changed, 84 insertions(+), 9 deletions(-) create mode 100644 APMrover2/mode_acro.cpp diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 96ecdeac56196..4cfca7f89b8f7 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -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 }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 6f5722e07d0c5..9af6e0653e400 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -316,6 +316,9 @@ class ParametersG2 { // 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[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index b4342201c7f29..eb56a9f3c77bc 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -104,6 +104,7 @@ class Rover : public AP_HAL::HAL::Callbacks { #endif friend class GCS_Rover; friend class Mode; + friend class ModeAcro; friend class ModeAuto; friend class ModeGuided; friend class ModeHold; @@ -378,6 +379,7 @@ class Rover : public AP_HAL::HAL::Callbacks { ModeInitializing mode_initializing; ModeHold mode_hold; ModeManual mode_manual; + ModeAcro mode_acro; ModeGuided mode_guided; ModeAuto mode_auto; ModeSteering mode_steering; diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index bc2a27fa6d409..466097d3bbeee 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -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 = {}; diff --git a/APMrover2/defines.h b/APMrover2/defines.h index e7b33fff036b6..6f67349078c55 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -28,6 +28,7 @@ enum ch7_option { // ---------------- enum mode { MANUAL = 0, + ACRO = 1, STEERING = 3, HOLD = 4, AUTO = 10, diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 56bb07b9910c9..64dbaf4135f81 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -145,6 +145,20 @@ class Mode }; +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: diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp new file mode 100644 index 0000000000000..70a57d6ea5a34 --- /dev/null +++ b/APMrover2/mode_acro.cpp @@ -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); + } +} diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 2576564473dbc..8d059141fa836 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -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;