From c216bac6c4eac3ced52ba5a4a1a3917c9dc1d00c Mon Sep 17 00:00:00 2001 From: Andy Little Date: Thu, 29 Feb 2024 17:31:57 +0000 Subject: [PATCH] Sailboat: In Irons : Tidy up comments --- Rover/mode.cpp | 9 ++++----- Rover/sailboat.cpp | 48 +++++++++++++++++++++------------------------- Rover/sailboat.h | 3 +++ 3 files changed, 29 insertions(+), 31 deletions(-) diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 63d0db1376b66e..4a21d3b7666434 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -415,11 +415,10 @@ float Mode::calc_speed_nudge(float target_speed, bool reversed) void Mode::navigate_to_waypoint() { if ( g2.sailboat.in_irons()){ - // rudder_percent ( 0.0f to 1.0f) sets the rudder angle used to turn the boat - // as it is blown backwards. Too much rudder angle can stall the rudders - // and slow the process of getting out of irons - - //set rudder output to turn the boat in required direction + // set rudder output to turn the boat in required direction + // rudder_percent ( 0.0f to 1.0f) sets the rudder angle used to turn the boat + // as it is blown backwards. Too much rudder angle can stall the rudders + // and slow the process of getting out of irons g2.motors.set_steering(g2.sailboat.get_in_irons_rudder() * 4500.0f,false); return; } diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index 77a50f994b0035..b6ecce14d64fa4 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -4,7 +4,7 @@ #define SAILBOAT_TACKING_ACCURACY_DEG 10 // tack is considered complete when vehicle is within this many degrees of target tack angle #define SAILBOAT_NOGO_PAD 10 // deg, the no go zone is padded by this much when deciding if we should use the Sailboat heading controller #define TACK_RETRY_TIME_MS 5000 // Can only try another auto mode tack this many milliseconds after the last is cleared (either competed or timed-out) -#define IN_IRONS_LIMBO_MS 250 +#define IN_IRONS_LIMBO_MS 250 // Time spent going backwards before in irons is true /* To Do List - Improve tacking in light winds and bearing away in strong wings @@ -106,17 +106,11 @@ const AP_Param::GroupInfo Sailboat::var_info[] = { // @Param: OPTIONS // @DisplayName: Sailboat options // @Description: Sailboat setup options, Only available for separate mainsail RC channel. - /* @bitmask: 0:TODO Mainsail works as traditional RC sail in manual. - Range 1000 usec to 2000 usec. - 1000 usec is fully sheeted in, 2000 usec is fully sheeted out */ + // @bitmask: 0:TODO Mainsail works as traditional RC sail in manual. Range 1000 usec to 2000 usec. 1000 usec is fully sheeted in, 2000 usec is fully sheeted out // @bitmask: 1:reserved // @bitmask: 2:reserved // @bitmask: 3:reserved - /* @bitmask: 4:TODO mainsail channel modifies angle of attack in auto, acro. - MainSailRCIn range {1000 to 1900} usec: - Sail angle of attack = (1.0f - (MainsailChannelIn - 1000.0f)/900.0f) * - SAIL_ANGLE_IDEAL. - MainSailRCIn range {1900 +} usec: Sail relaxed. */ + // @bitmask: 4:TODO mainsail channel modifies angle of attack in auto, acro. MainSailRCIn range {1000 to 1900} usec: Sail angle of attack = (1.0f - (MainsailChannelIn - 1000.0f)/900.0f) * SAIL_ANGLE_IDEAL. MainSailRCIn range {1900 +} usec: Sail relaxed. // @bitmask: 5:reserved // @bitmask: 6:reserved // @bitmask: 7:Detect and get out of "in Irons" in AUTO mode @@ -427,9 +421,9 @@ bool Sailboat::use_indirect_route(float desired_heading_cd) const float Sailboat::calc_heading(float desired_heading_cd) { if ( in_irons()){ - // in irons dont change the best heading, - // keep slowly rotating in one direction - return degrees(in_irons_heading_rad) * 100.0f ; + // in irons dont change the best heading, + // keep slowly rotating in one direction + return degrees(in_irons_heading_rad) * 100.0f ; } if (!tack_enabled()) { @@ -595,6 +589,14 @@ bool Sailboat::motor_assist_low_wind() const bool Sailboat::in_irons() { + // only enabled for separate mainsail and options bit set + if (! get_out_of_in_irons_in_auto_enabled()){ + clear_in_irons(); + return false; + } + // helper lambda functions + + // do low level set in irons work auto set_in_irons = [this]() { // fix the desired heading while in irons @@ -622,10 +624,6 @@ bool Sailboat::in_irons() (in_irons_rotate_sign == Sailboat::rotate_clockwise)? "port":"stbd"); }; - if (! get_out_of_in_irons_in_auto_enabled()){ - clear_in_irons(); - return false; - } auto going_backwards = []()-> bool{ float speed; // return false if we don't have a valid speed (e.g no gps fix) @@ -648,35 +646,33 @@ bool Sailboat::in_irons() }; // Already "in irons" or not? - //if (!rover.g2.motors.sailboat_in_irons()){ - if (! is_in_irons){ + if (!is_in_irons){ // not currently in irons, check conditions to enter "in irons limbo" - if ( cannot_motorsail() && going_backwards() && headed() ){ + if (cannot_motorsail() && going_backwards() && headed()){ // in_irons_limbo_start_ms == 0 means not yet "in irons limbo" - if ( in_irons_start_ms == 0){ + if (in_irons_start_ms == 0){ // start "in irons limbo" in_irons_start_ms = millis(); - }else{ + } else { // we have been in the "in irons limbo" period for a while - if ( ( millis() - in_irons_start_ms ) >= IN_IRONS_LIMBO_MS){ + if ((millis() - in_irons_start_ms) >= IN_IRONS_LIMBO_MS){ // OK, long enough set_in_irons(); } } - }else { // not going backwards (anymore), so reset limbo timer. + } else { // not going backwards (anymore), so reset limbo timer. in_irons_start_ms = 0; } } else { // currently "in irons". Has boat rotated enough to // be able to sheet in, fill sails and Go? - if ( abs(wind_dir_apparent) > sail_no_go){ + if (abs(wind_dir_apparent) > sail_no_go){ in_irons_start_ms = 0; is_in_irons = false; - //rover.g2.motors.clear_sailboat_in_irons(); gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Out of Irons"); } } - return is_in_irons ; //rover.g2.motors.sailboat_in_irons(); + return is_in_irons ; } void Sailboat::clear_in_irons() diff --git a/Rover/sailboat.h b/Rover/sailboat.h index 54b905c82f45ce..4c57afa079ef51 100644 --- a/Rover/sailboat.h +++ b/Rover/sailboat.h @@ -92,6 +92,7 @@ class Sailboat // state machine. the state is updated at each call // return true if sailboat is stuck "in irons" bool in_irons(); + // return the rudder to set to get out of irons // between -1 and 1 float get_in_irons_rudder() const { return in_irons_rudder;} @@ -100,8 +101,10 @@ class Sailboat // Usually in_irons will clear itself by getting out of irons // Can be called whether sailboat is in_irons or not void clear_in_irons(); + // is get out of in irons in auto mode enabled? bool get_out_of_in_irons_in_auto_enabled() const; + // is mainsail on a separate rc in channel? bool separate_mainsail_rcin_channel() const; private: