Rover: direct-rotation sail mast control

Rover/AP_MotorsUGV.cpp

@@ -182,6 +182,8 @@ void AP_MotorsUGV::setup_servo_output()
     SRV_Channels::set_range(SRV_Channel::k_mainsail_sheet, 100);
     // wing sail -100 to 100
     SRV_Channels::set_angle(SRV_Channel::k_wingsail_elevator, 100);
+    // mast rotation -100 to 100
+    SRV_Channels::set_angle(SRV_Channel::k_mast_rotation, 100);
 
 }
 
@@ -242,6 +244,12 @@ void AP_MotorsUGV::set_wingsail(float wingsail)
     _wingsail = constrain_float(wingsail, -100.0f, 100.0f);
 }
 
+// set mast rotation input as a value from -100 to 100
+void AP_MotorsUGV::set_mast_rotation(float mast_rotation)
+{
+    _mast_rotation = constrain_float(mast_rotation, -100.0f, 100.0f);
+}
+
 // get slew limited throttle
 // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
 // same as private slew_limit_throttle method (see below) but does not update throttle state
@@ -270,7 +278,7 @@ bool AP_MotorsUGV::have_skid_steering() const
 // true if the vehicle has a mainsail
 bool AP_MotorsUGV::has_sail() const
 {
-    return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet) || SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator);
+    return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet) || SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator) || SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation);
 }
 
 void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
@@ -360,6 +368,9 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)
             if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) {
                 SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, pct);
             }
+            if (SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation)) {
+                SRV_Channels::set_output_scaled(SRV_Channel::k_mast_rotation, pct);
+            }
             break;
         }
         case MOTOR_TEST_LAST:
@@ -423,6 +434,9 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm)
             if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) {
                 SRV_Channels::set_output_pwm(SRV_Channel::k_wingsail_elevator, pwm);
             }
+            if (SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation)) {
+                SRV_Channels::set_output_pwm(SRV_Channel::k_mast_rotation, pwm);
+            }
             break;
         }
         default:
@@ -885,6 +899,7 @@ void AP_MotorsUGV::output_sail()
 
     SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, _mainsail);
     SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, _wingsail);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_mast_rotation, _mast_rotation);
 }
 
 // slew limit throttle for one iteration

Rover/AP_MotorsUGV.h

@@ -84,6 +84,10 @@ public:
     void set_wingsail(float wingsail);
     float get_wingsail() const { return _wingsail; }
 
+    // set or get mast rotation input as a value from -100 to 100
+    void set_mast_rotation(float mast_rotation);
+    float get_mast_rotation() const { return _mast_rotation; }
+
     // get slew limited throttle
     // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
     // same as private slew_limit_throttle method (see below) but does not update throttle state
@@ -199,6 +203,7 @@ protected:
     float   _walking_height; // requested height as a value from -1 to +1   
     float   _mainsail;  // requested mainsail input as a value from 0 to 100
     float   _wingsail;  // requested wing sail input as a value in the range +- 100
+    float   _mast_rotation;  // requested mast rotation input as a value in the range +- 100
 
     // omni variables
     float   _throttle_factor[AP_MOTORS_NUM_MOTORS_MAX];

Rover/mode.cpp

@@ -307,9 +307,11 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
         // sailboats use special throttle and mainsail controller
         float mainsail_out = 0.0f;
         float wingsail_out = 0.0f;
-        rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out, wingsail_out);
+        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()) {

Rover/mode_manual.cpp

@@ -29,9 +29,11 @@ void ModeManual::update()
     // set sailboat sails
     float desired_mainsail;
     float desired_wingsail;
-    g2.sailboat.get_pilot_desired_mainsail(desired_mainsail, 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

@@ -41,7 +41,7 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
 
     // @Param: ANGLE_MAX
     // @DisplayName: Sail max angle
-    // @Description: Mainsheet loose, angle between centerline and boom
+    // @Description: Mainsheet loose, angle between centerline and boom. For direct-control rotating masts, the rotation angle at SERVOx_MAX/_MIN; for rotating masts, this value can exceed 90 degrees if the linkages can physically rotate the mast past that angle.
     // @Units: deg
     // @Range: 0 90
     // @Increment: 1
@@ -170,26 +170,29 @@ void Sailboat::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
-void Sailboat::get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out)
+void Sailboat::get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out, float &mast_rotation_out)
 {
     // no RC input means mainsail is moved to trim
     if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) {
         mainsail_out = 100.0f;
         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)
+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;
     }
 
@@ -212,12 +215,17 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
     if (motor_state == UseMotor::USE_MOTOR_ALWAYS) {
         mainsail_out = 100.0f;
         wingsail_out = 0.0f;
+        mast_rotation_out = 0.0f;
         return;
     }
 
-    // use PID controller to sheet out
+    // use PID controller to sheet out, this number is expected approximately in the 0 to 100 range (with default PIDs)
-    float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
+    const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
-    pid_offset = constrain_float(pid_offset, 0.0f, 100.0f);
+
+    // get apparent wind, + is wind over starboard side, - is wind over port side
+    const float wind_dir_apparent = degrees(rover.g2.windvane.get_apparent_wind_direction_rad());
+    const float wind_dir_apparent_abs = fabsf(wind_dir_apparent);
+    const float wind_dir_apparent_sign = is_negative(wind_dir_apparent) ? -1.0f : 1.0f;
 
     //
     // mainsail control
@@ -227,12 +235,10 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
     if (!is_positive(desired_speed)) {
         mainsail_out = 100.0f;
     } else {
-        // + is wind over starboard side, - is wind over port side, but as the sails are sheeted the same on each side it makes no difference so take abs
+        // Sails are sheeted the same on each side use abs wind direction
-        float wind_dir_apparent = fabsf(rover.g2.windvane.get_apparent_wind_direction_rad());
-        wind_dir_apparent = degrees(wind_dir_apparent);
 
         // set the main sail to the ideal angle to the wind
-        float mainsail_angle = wind_dir_apparent -sail_angle_ideal;
+        float mainsail_angle = wind_dir_apparent_abs - sail_angle_ideal;
 
         // make sure between allowable range
         mainsail_angle = constrain_float(mainsail_angle,sail_angle_min, sail_angle_max);
@@ -247,17 +253,58 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
     // wingsail control
     //
 
-    // wing sails auto trim, we only need to reduce power if we are tipping over
+    // wing sails auto trim, we only need to reduce power if we are tipping over, must also be trimmed for correct tack
-    wingsail_out = 100.0f - pid_offset;
+    // 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 must be trimmed for the correct tack
+    // wing sails can be used to go backwards, probably not recommended though
-    if (rover.g2.windvane.get_current_tack() == AP_WindVane::Sailboat_Tack::TACK_PORT) {
+    if (is_negative(desired_speed)) {
         wingsail_out *= -1.0f;
     }
 
-    // wing sails can be used to go backwards, probably not recommended though
+    //
+    // direct mast rotation control
+    //
+
     if (!is_positive(desired_speed)) {
-        wingsail_out *= -1.0f;
+        // 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
+                // multiply pid_offset by 0.01 to keep the scaling in the same range as the other sail outputs
+                // this means the default PIDs should apply reasonably well to all sail types
+                mast_rotation_angle = wind_dir_apparent_abs - sail_angle_ideal * MAX(1.0f - pid_offset*0.01f,0.0f);
+
+                // restore sign
+                mast_rotation_angle *= wind_dir_apparent_sign;
+
+            } else {
+                // use sail as drag device, but avoid wagging the sail as the wind oscillates
+                // between 180 and -180 degrees
+                mast_rotation_angle = 90.0f;
+                if (wind_dir_apparent_abs > 135.0f) {
+                    // wind is almost directly behind, keep wing on current tack
+                    if (SRV_Channels::get_output_scaled(SRV_Channel::k_mast_rotation) < 0) {
+                        mast_rotation_angle *= -1.0f;
+                    }
+                } else {
+                    // set the wing on the correct tack, so that is can be sheeted in if required
+                    mast_rotation_angle *= wind_dir_apparent_sign;
+                }
+            }
+
+            // linear interpolate servo displacement (-100 to 100) from mast rotation angle and restore sign
+            mast_rotation_out = linear_interpolate(-100.0f, 100.0f, mast_rotation_angle, -sail_angle_max, sail_angle_max);
+        }
     }
 
 }

Rover/sailboat.h

@@ -38,10 +38,11 @@ public:
     // 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
-    void get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out);
+    // 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);
+    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;