From 6ba5942ecec0d3e049f0599b8f5174be60ab407c Mon Sep 17 00:00:00 2001 From: Ammarf <35584084+Ammarf@users.noreply.github.com> Date: Mon, 2 Jul 2018 16:21:37 +0900 Subject: [PATCH] Rover: add simple mode --- APMrover2/Parameters.cpp | 22 ++++++++++++++------- APMrover2/Parameters.h | 3 +++ APMrover2/RC_Channel.cpp | 8 ++++++++ APMrover2/Rover.h | 6 ++++++ APMrover2/mode.cpp | 24 +++++++++++++++++++++++ APMrover2/mode.h | 27 ++++++++++++++++++++++++++ APMrover2/mode_simple.cpp | 41 +++++++++++++++++++++++++++++++++++++++ APMrover2/system.cpp | 3 +++ 8 files changed, 127 insertions(+), 7 deletions(-) create mode 100644 APMrover2/mode_simple.cpp diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index b1d3f6b3ff..66915de8ab 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -33,7 +33,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,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), @@ -211,7 +211,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: MODE1 // @DisplayName: Mode1 - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049) GSCALAR(mode1, "MODE1", Mode::Number::MANUAL), @@ -219,35 +219,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,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode2, "MODE2", Mode::Number::MANUAL), // @Param: MODE3 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode3, "MODE3", Mode::Number::MANUAL), // @Param: MODE4 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode4, "MODE4", Mode::Number::MANUAL), // @Param: MODE5 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode5, "MODE5", Mode::Number::MANUAL), // @Param: MODE6 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode6, "MODE6", Mode::Number::MANUAL), @@ -591,6 +591,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(rally, "RALLY_", 28, ParametersG2, AP_Rally_Rover), #endif + // @Param: SIMPLE_TYPE + // @DisplayName: Simple_Type + // @Description: Simple mode types + // @Values: 0:InitialHeading,1:CardinalDirections + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("SIMPLE_TYPE", 29, ParametersG2, simple_type, 0), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index e2a014663f..6d8fd12b33 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -367,6 +367,9 @@ public: // Rally point library AP_Rally_Rover rally; + + // Simple mode types + AP_Int8 simple_type; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/RC_Channel.cpp b/APMrover2/RC_Channel.cpp index e30e0ea40d..4e66a9e29c 100644 --- a/APMrover2/RC_Channel.cpp +++ b/APMrover2/RC_Channel.cpp @@ -32,6 +32,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::FOLLOW: ret = &mode_follow; break; + case Mode::Number::SIMPLE: + ret = &mode_simple; + break; case Mode::Number::AUTO: ret = &mode_auto; break; @@ -226,6 +229,11 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi do_aux_function_change_mode(rover.mode_follow, ch_flag); break; + // set mode to Simple + case SIMPLE: + do_aux_function_change_mode(rover.mode_simple, ch_flag); + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index f08704e2ef..719a15b365 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -121,6 +121,7 @@ public: friend class ModeRTL; friend class ModeSmartRTL; friend class ModeFollow; + friend class ModeSimple; friend class RC_Channel_Rover; friend class RC_Channels_Rover; @@ -376,6 +377,7 @@ private: ModeRTL mode_rtl; ModeSmartRTL mode_smartrtl; ModeFollow mode_follow; + ModeSimple mode_simple; // cruise throttle and speed learning struct { @@ -397,6 +399,7 @@ private: void one_second_loop(void); void update_GPS(void); void update_current_mode(void); + void init_simple_mode(void); // balance_bot.cpp void balancebot_pitch_control(float &throttle); @@ -576,6 +579,9 @@ public: // frame type uint8_t get_frame_type() { return g2.frame_type.get(); } AP_WheelRateControl& get_wheel_rate_control() { return g2.wheel_rate_control; } + + // Simple mode + float simple_sin_yaw; }; extern const AP_HAL::HAL& hal; diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 2b81898904..beb9828303 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -145,6 +145,30 @@ void Mode::get_pilot_desired_lateral(float &lateral_out) lateral_out = rover.channel_lateral->get_control_in(); } +// decode pilot's input and return heading_out (in cd) and speed_out (in m/s) +void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out) +{ + float desired_steering; + float desired_throttle; + get_pilot_input(desired_steering, desired_throttle); + + // scale down and limit throttle and steering to a -1 to +1 range + desired_throttle = constrain_float(desired_throttle / 100.0f, -1.0f, 1.0f); + desired_steering = constrain_float(desired_steering / 4500.0f, -1.0, 1.0f); + + // calculate angle of input stick vector + heading_out = wrap_360_cd(atan2f(desired_steering,desired_throttle) * DEGX100); + + // calculate magnitude of input stick vector + float magnitude_max = 1.0f; + float magnitude = safe_sqrt(sq(desired_throttle) + sq(desired_steering)); + if (magnitude > magnitude_max) { + magnitude = magnitude_max; + } + float throttle = magnitude / magnitude_max; + speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); +} + // set desired location void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) { diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 51c5698af7..c6d3117614 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -25,6 +25,7 @@ public: HOLD = 4, LOITER = 5, FOLLOW = 6, + SIMPLE = 7, AUTO = 10, RTL = 11, SMART_RTL = 12, @@ -134,6 +135,9 @@ protected: // decode pilot lateral movement input and return in lateral_out argument void get_pilot_desired_lateral(float &lateral_out); + // decode pilot's input and return heading_out (in cd) and speed_out (in m/s) + void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out); + // calculate steering output to drive along line from origin to destination waypoint void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false); @@ -482,3 +486,26 @@ protected: bool _enter() override; }; + +class ModeSimple : public Mode +{ +public: + + uint32_t mode_number() const override { return SIMPLE; } + const char *name4() const override { return "SMPL"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + void init_simple_heading(); + +protected: + + float simple_initial_heading; + + // simple type enum used for SIMPLE_TYPE parameter + enum simple_type { + Simple_InitialHeading = 0, + Simple_CardinalDirections = 1, + }; +}; + diff --git a/APMrover2/mode_simple.cpp b/APMrover2/mode_simple.cpp new file mode 100644 index 0000000000..e8c33bda2d --- /dev/null +++ b/APMrover2/mode_simple.cpp @@ -0,0 +1,41 @@ +#include "mode.h" +#include "Rover.h" + +void ModeSimple::init_simple_heading() +{ + simple_initial_heading = ahrs.yaw; +} + +void ModeSimple::update() +{ + float desired_heading, desired_steering, desired_speed; + + // initial heading simple mode + if (g2.simple_type == Simple_InitialHeading) { + + // get piot input + get_pilot_desired_steering_and_speed(desired_steering, desired_speed); + + float simple_heading; + if (is_zero(desired_steering)) { + simple_heading = ((simple_initial_heading - ahrs.yaw) * 4500.0f); + } else { + simple_heading = desired_steering; + } + + // run throttle and steering controllers + calc_steering_to_heading(simple_heading, false); + calc_throttle(desired_speed, true, false); + } + + // cardinal directions simple mode + if (g2.simple_type == Simple_CardinalDirections) { + + // get pilot input + get_pilot_desired_heading_and_speed(desired_heading, desired_speed); + + // run throttle and steering controllers + calc_steering_to_heading(desired_heading, false); + calc_throttle(desired_speed, false, true); + } +} diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index af03b9618d..8fd37fa249 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -332,6 +332,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method) // Set the SmartRTL home location. If activated, SmartRTL will ultimately try to land at this point g2.smart_rtl.set_home(true); + // initialize simple mode heading + rover.mode_simple.init_simple_heading(); + change_arm_state(); return true; }