Rover Sailboat: refactor the mainsail/wingmast/mast_rotation logic and make non const sailboat functions private to the sailboat class.

Saves around 128 bytes of text image size (in SITL anyway)

Rover/mode.cpp

@@ -304,13 +304,7 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
 
     if (rover.g2.sailboat.sail_enabled()) {
         // sailboats use special throttle and mainsail controller
-        float mainsail_out = 0.0f;
+        rover.g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out);
-        float wingsail_out = 0.0f;
-        float mast_rotation_out = 0.0f;
-        rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out, wingsail_out, mast_rotation_out);
-        rover.g2.motors.set_mainsail(mainsail_out);
-        rover.g2.motors.set_wingsail(wingsail_out);
-        rover.g2.motors.set_mast_rotation(mast_rotation_out);
     } else {
         // call speed or stop controller
         if (is_zero(target_speed) && !rover.is_balancebot()) {
@@ -348,8 +342,7 @@ bool Mode::stop_vehicle()
     }
 
     // relax sails if present
-    g2.motors.set_mainsail(100.0f);
+    g2.sailboat.relax_sails();
-    g2.motors.set_wingsail(0.0f);
 
     // send to motor
     g2.motors.set_throttle(throttle_out);

Rover/mode_hold.cpp

@@ -10,8 +10,7 @@ void ModeHold::update()
     }
 
     // relax mainsail
-    g2.motors.set_mainsail(100.0f);
+    g2.sailboat.relax_sails();
-    g2.motors.set_wingsail(0.0f);
 
     // hold position - stop motors and center steering
     g2.motors.set_throttle(throttle);

Rover/mode_manual.cpp

@@ -29,13 +29,7 @@ void ModeManual::update()
     g2.motors.set_walking_height(desired_walking_height);
 
     // set sailboat sails
-    float desired_mainsail;
+    g2.sailboat.set_pilot_desired_mainsail();
-    float desired_wingsail;
-    float desired_mast_rotation;
-    g2.sailboat.get_pilot_desired_mainsail(desired_mainsail, desired_wingsail, desired_mast_rotation);
-    g2.motors.set_mainsail(desired_mainsail);
-    g2.motors.set_wingsail(desired_wingsail);
-    g2.motors.set_mast_rotation(desired_wingsail);
 
     // copy RC scaled inputs to outputs
     g2.motors.set_throttle(desired_throttle);

Rover/sailboat.cpp

@@ -172,57 +172,24 @@ void Sailboat::init_rc_in()
     }
 }
 
-// decode pilot mainsail input and return in steer_out and throttle_out arguments
+// @brief decode pilot mainsail input in manual modes and update the various sail values for different sail types
-// mainsail_out is in the range 0 to 100, defaults to 100 (fully relaxed) if no input configured
+// ready for SRV_Channel output.
-void Sailboat::get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out, float &mast_rotation_out)
+void Sailboat::set_pilot_desired_mainsail()
 {
     // no RC input means mainsail is moved to trim
     if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) {
-        mainsail_out = 100.0f;
+       relax_sails();
-        wingsail_out = 0.0f;
-        mast_rotation_out = 0.0f;
-        return;
-    }
-    mainsail_out = constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f);
-    wingsail_out = constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f);
-    mast_rotation_out = constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f);
-}
-
-// calculate throttle and mainsail angle required to attain desired speed (in m/s)
-// returns true if successful, false if sailboats not enabled
-void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttle_out, float &mainsail_out, float &wingsail_out, float &mast_rotation_out)
-{
-    if (!sail_enabled()) {
-        throttle_out = 0.0f;
-        mainsail_out = 0.0f;
-        wingsail_out = 0.0f;
-        mast_rotation_out = 0.0f;
-        return;
-    }
-
-    // run speed controller if motor is forced on or motor assistance is required for low speeds or tacking
-    if ((motor_state == UseMotor::USE_MOTOR_ALWAYS) ||
-         motor_assist_tack() ||
-         motor_assist_low_wind()) {
-        // run speed controller - duplicate of calls found in mode::calc_throttle();
-        throttle_out = 100.0f * rover.g2.attitude_control.get_throttle_out_speed(desired_speed,
-                                                                        rover.g2.motors.limit.throttle_lower,
-                                                                        rover.g2.motors.limit.throttle_upper,
-                                                                        rover.g.speed_cruise,
-                                                                        rover.g.throttle_cruise * 0.01f,
-                                                                        rover.G_Dt);
     }else{
-        throttle_out = 0.0f;
+       rover.g2.motors.set_mainsail(constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f));
-    }
+       rover.g2.motors.set_wingsail(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f));
-
+       rover.g2.motors.set_mast_rotation(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f));
-    // if we are motoring relax sails
+    }
-    if (motor_state == UseMotor::USE_MOTOR_ALWAYS) {
-        mainsail_out = 100.0f;
-        wingsail_out = 0.0f;
-        mast_rotation_out = 0.0f;
-        return;
 }
 
+/// @brief Set mainsail in auto modes
+/// @param[in] desired_speed desired speed (in m/s) only used to detect desired direction
+void Sailboat::set_auto_mainsail(float desired_speed)
+{
     // use PID controller to sheet out, this number is expected approximately in the 0 to 100 range (with default PIDs)
     const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
 
@@ -232,55 +199,41 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
     const float wind_dir_apparent_sign = is_negative(wind_dir_apparent) ? -1.0f : 1.0f;
 
     //
-    // mainsail control
+    // mainsail control.
     //
-
+    // mainsail_out represents a range from 0 to 100
+    float mainsail_out = 100.f;
     // main sails cannot be used to reverse
-    if (!is_positive(desired_speed)) {
+    if (is_positive(desired_speed)) {
-        mainsail_out = 100.0f;
-    } else {
         // Sails are sheeted the same on each side use abs wind direction
-
         // set the main sail to the ideal angle to the wind
-        float mainsail_angle = wind_dir_apparent_abs - sail_angle_ideal;
+        const float mainsail_angle =
-
+            constrain_float(wind_dir_apparent_abs - sail_angle_ideal,sail_angle_min, sail_angle_max);
-        // make sure between allowable range
-        mainsail_angle = constrain_float(mainsail_angle,sail_angle_min, sail_angle_max);
 
         // linear interpolate mainsail value (0 to 100) from wind angle mainsail_angle
-        float mainsail_base = linear_interpolate(0.0f, 100.0f, mainsail_angle,sail_angle_min,sail_angle_max);
+        const float mainsail_base = linear_interpolate(0.0f, 100.0f, mainsail_angle,sail_angle_min,sail_angle_max);
 
         mainsail_out = constrain_float((mainsail_base + pid_offset), 0.0f ,100.0f);
     }
-
+    rover.g2.motors.set_mainsail(mainsail_out);
     //
     // wingsail control
-    //
-
     // wing sails auto trim, we only need to reduce power if we are tipping over, must also be trimmed for correct tack
     // dont allow to reduce power to less than 0, ie not backwinding the sail to self-right
-    wingsail_out = (100.0f - MIN(pid_offset,100.0f)) * wind_dir_apparent_sign;
-
     // wing sails can be used to go backwards, probably not recommended though
-    if (is_negative(desired_speed)) {
+    const float wing_sail_out_sign = is_negative(desired_speed)?-1.f:1.f;
-        wingsail_out *= -1.0f;
+    const float wingsail_out = (100.0f - MIN(pid_offset,100.0f)) * wind_dir_apparent_sign * wing_sail_out_sign;
-    }
+    rover.g2.motors.set_wingsail(wingsail_out);
-
     //
     // direct mast rotation control
     //
-
+    float mast_rotation_out = 0.f;
-    if (!is_positive(desired_speed)) {
+    if (is_positive(desired_speed)) {
         // rotating sails can be used to reverse, but not in this version
-        mast_rotation_out = 0.0f;
-    } else {
-
         if (wind_dir_apparent_abs < sail_angle_ideal) {
             // in irons, center the sail.
             mast_rotation_out = 0.0f;
-
         } else {
-
             float mast_rotation_angle;
             if (wind_dir_apparent_abs < (90.0f + sail_angle_ideal)) {
                 // use sail as a lift device, at ideal angle of attack, but depower to prevent excessive heel
@@ -310,7 +263,45 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
             mast_rotation_out = linear_interpolate(-100.0f, 100.0f, mast_rotation_angle, -sail_angle_max, sail_angle_max);
         }
     }
+    rover.g2.motors.set_mast_rotation(mast_rotation_out);
+}
 
+void Sailboat::relax_sails()
+{
+   rover.g2.motors.set_mainsail(100.0f);
+   rover.g2.motors.set_wingsail(0.f);
+   rover.g2.motors.set_mast_rotation(0.f);
+}
+
+// calculate throttle and mainsail angle required to attain desired speed (in m/s)
+void Sailboat::get_throttle_and_set_mainsail(float desired_speed, float &throttle_out)
+{
+    if (!sail_enabled()) {
+        relax_sails();
+        throttle_out = 0.0f;
+        return;
+    }
+
+    // run speed controller if motor is forced on or motor assistance is required for low speeds or tacking
+    if ((motor_state == UseMotor::USE_MOTOR_ALWAYS) ||
+         motor_assist_tack() ||
+         motor_assist_low_wind()) {
+        // run speed controller - duplicate of calls found in mode::calc_throttle();
+        throttle_out = 100.0f * rover.g2.attitude_control.get_throttle_out_speed(desired_speed,
+                                                                        rover.g2.motors.limit.throttle_lower,
+                                                                        rover.g2.motors.limit.throttle_upper,
+                                                                        rover.g.speed_cruise,
+                                                                        rover.g.throttle_cruise * 0.01f,
+                                                                        rover.G_Dt);
+    } else {
+        throttle_out = 0.0f;
+    }
+
+    if (motor_state == UseMotor::USE_MOTOR_ALWAYS) {
+       relax_sails();
+    }else{
+       set_auto_mainsail(desired_speed);
+    }
 }
 
 // Velocity Made Good, this is the speed we are traveling towards the desired destination

Rover/sailboat.h

@@ -18,6 +18,15 @@
 */
 class Sailboat
 {
+
+   friend class Rover;
+   friend class Mode;
+   friend class ModeManual;
+   friend class ModeHold;
+   friend class ModeLoiter;
+   friend class ModeAcro;
+   friend class RC_Channel_Rover;
+
 public:
 
     // constructor
@@ -29,45 +38,15 @@ public:
     // true if sailboat navigation (aka tacking) is enabled
     bool tack_enabled() const;
 
-    // setup
-    void init();
-
-    // initialise rc input (channel_mainsail)
-    void init_rc_in();
-
-    // decode pilot mainsail input and return in steer_out and throttle_out arguments
-    // mainsail_out is in the range 0 to 100, defaults to 100 (fully relaxed) if no input configured
-    // wingsail_out is in the range -100 to 100, defaults to 0
-    // mast_rotation_out is in the range -100 to 100, defaults to 0
-    void get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out, float &mast_rotation_out);
-
-    // calculate throttle and mainsail angle required to attain desired speed (in m/s)
-    void get_throttle_and_mainsail_out(float desired_speed, float &throttle_out, float &mainsail_out, float &wingsail_out, float &mast_rotation_out);
-
     // Velocity Made Good, this is the speed we are traveling towards the desired destination
     float get_VMG() const;
 
-    // handle user initiated tack while in acro mode
-    void handle_tack_request_acro();
-
-    // return target heading in radians when tacking (only used in acro)
-    float get_tack_heading_rad();
-
-    // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
-    void handle_tack_request_auto();
-
-    // clear tacking state variables
-    void clear_tack();
-
     // returns true if boat is currently tacking
     bool tacking() const;
 
     // returns true if sailboat should take a indirect navigation route to go upwind
     bool use_indirect_route(float desired_heading_cd) const;
 
-    // calculate the heading to sail on if we cant go upwind
-    float calc_heading(float desired_heading_cd);
-
     // states of USE_MOTOR parameter and motor_state variable
     enum class UseMotor {
         USE_MOTOR_NEVER  = 0,
@@ -75,18 +54,48 @@ public:
         USE_MOTOR_ALWAYS = 2
     };
 
-    // set state of motor
+    // return sailboat loiter radius
-    // if report_failure is true a message will be sent to all GCSs
+    float get_loiter_radius() const {return loit_radius;}
-    void set_motor_state(UseMotor state, bool report_failure = true);
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
-    // return sailboat loiter radius
-    float get_loiter_radius() const {return loit_radius;}
-
 private:
 
+    // handle user initiated tack while in acro mode
+    void handle_tack_request_acro();
+
+  // setup
+    void init();
+
+    // initialise rc input (channel_mainsail)
+    void init_rc_in();
+
+
+    // return target heading in radians when tacking (only used in acro)
+    float get_tack_heading_rad();
+
+    // clear tacking state variables
+    void clear_tack();
+
+    // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
+    void handle_tack_request_auto();
+
+    // set state of motor
+    // if report_failure is true a message will be sent to all GCSs
+    void set_motor_state(UseMotor state, bool report_failure = true);
+
+        // calculate the heading to sail on if we cant go upwind
+    float calc_heading(float desired_heading_cd);
+
+    void set_pilot_desired_mainsail();
+
+    void set_auto_mainsail(float desired_speed);
+    void relax_sails();
+
+    // calculate throttle and set sail
+    void get_throttle_and_set_mainsail(float desired_speed, float &throttle_out);
+
     // true if motor is on to assist with slow tack
     bool motor_assist_tack() const;