Rover: rework sailboats with motoring

renamed throttle with motor
added set_motor_state accessor
removed overloading of ENABLE parameter
fixed circular tack_enabled logic which led to assisting a tack disabling a tack
separate assistance for low wind vs tacking

APMrover2/sailboat.cpp

@@ -25,7 +25,7 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
     // @Param: ENABLE
     // @DisplayName: Enable Sailboat
     // @Description: This enables Sailboat functionality
-    // @Values: 0:Disable,1:Enable sail assist only,2:Enable
+    // @Values: 0:Disable,1:Enable
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("ENABLE", 1, Sailboat, enable, 0, AP_PARAM_FLAG_ENABLE),
@@ -82,7 +82,7 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
     // @Range: 0 5
     // @Increment: 0.1
     // @User: Standard
-    AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_assist_windspeed, 0),
+    AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_windspeed_min, 0),
 
     AP_GROUPEND
 };
@@ -96,12 +96,26 @@ Sailboat::Sailboat()
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// Should we use sailboat navigation?
+// true if sailboat navigation (aka tacking) is enabled
 bool Sailboat::tack_enabled() const
 {
-    return (enable == 2) &&
+    // tacking disabled if not a sailboat
-           (throttle_state != Sailboat_Throttle::FORCE_MOTOR) &&
+    if (!sail_enabled()) {
-           (!throttle_assist() || tack_assist);
+        return false;
+    }
+
+    // tacking disabled if motor is always on
+    if (motor_state == UseMotor::USE_MOTOR_ALWAYS) {
+        return false;
+    }
+
+    // disable tacking if motor is available and wind is below cutoff
+    if (motor_assist_low_wind()) {
+        return false;
+    }
+
+    // otherwise tacking is enabled
+    return true;
 }
 
 void Sailboat::init()
@@ -117,12 +131,9 @@ void Sailboat::init()
         rover.g2.wp_nav.set_default_overshoot(10);
     }
 
-    // if we have a throttle of some sort allow to use it
+    // initialise motor state to USE_MOTOR_ASSIST
-    if (rover.g2.motors.have_skid_steering() ||
+    // this will silently fail if there is no motor attached
-        SRV_Channels::function_assigned(SRV_Channel::k_throttle) ||
+    set_motor_state(UseMotor::USE_MOTOR_ASSIST, false);
-        rover.get_frame_type() != rover.g2.motors.frame_type::FRAME_TYPE_UNDEFINED) {
-        throttle_state = Sailboat_Throttle::ASSIST;
-    }
 }
 
 // initialise rc input (channel_mainsail), may be called intermittently
@@ -163,8 +174,10 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
         return;
     }
 
-    // run speed controller if motor is forced on or if assistance is enabled for low speeds or tacking
+    // run speed controller if motor is forced on or motor assistance is required for low speeds or tacking
-    if ((throttle_state == Sailboat_Throttle::FORCE_MOTOR) || throttle_assist()) {
+    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,
@@ -180,7 +193,7 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl
     // mainsail control
     //
     // if we are motoring or attempting to reverse relax the sail
-    if (throttle_state == Sailboat_Throttle::FORCE_MOTOR || !is_positive(desired_speed)) {
+    if (motor_state == UseMotor::USE_MOTOR_ALWAYS || !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
@@ -348,7 +361,7 @@ float Sailboat::calc_heading(float desired_heading_cd)
             clear_tack();
         } else if ((now - auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
             // tack has taken too long
-            if ((throttle_state == Sailboat_Throttle::ASSIST) && (now - auto_tack_start_ms) < (3.0f * SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) {
+            if ((motor_state == UseMotor::USE_MOTOR_ASSIST) && (now - auto_tack_start_ms) < (3.0f * SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) {
                 // if we have throttle available use it for another two time periods to get the tack done
                 tack_assist = true;
             } else {
@@ -368,25 +381,47 @@ float Sailboat::calc_heading(float desired_heading_cd)
     }
 }
 
-// should we use the throttle?
+// set state of motor
-bool Sailboat::throttle_assist() const
+void Sailboat::set_motor_state(UseMotor state, bool report_failure)
+{
+    // always allow motor to be disabled
+    if (state == UseMotor::USE_MOTOR_NEVER) {
+        motor_state = state;
+        return;
+    }
+
+    // enable assistance or always on if a motor is defined
+    if (rover.g2.motors.have_skid_steering() ||
+        SRV_Channels::function_assigned(SRV_Channel::k_throttle) ||
+        rover.get_frame_type() != rover.g2.motors.frame_type::FRAME_TYPE_UNDEFINED) {
+        motor_state = state;
+    } else if (report_failure) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Sailboat: failed to enable motor");
+    }
+}
+
+// true if motor is on to assist with slow tack
+bool Sailboat::motor_assist_tack() const
 {
     // throttle is assist is disabled
-    if (throttle_state != Sailboat_Throttle::ASSIST) {
+    if (motor_state != UseMotor::USE_MOTOR_ASSIST) {
         return false;
     }
 
-    // assist with a tack
+    // assist with a tack because it is taking too long
-    if (tack_assist) {
+    return tack_assist;
-        return true;
+}
-    }
+
-
+// true if motor should be on to assist with low wind
-    // wind speed is less than sailing cut-off
+bool Sailboat::motor_assist_low_wind() const
-    if (!is_zero(sail_assist_windspeed) &&
+{
-        rover.g2.windvane.wind_speed_enabled() &&
+    // motor assist is disabled
-        rover.g2.windvane.get_true_wind_speed() < sail_assist_windspeed) {
+    if (motor_state != UseMotor::USE_MOTOR_ASSIST) {
-        return true;
+        return false;
-    }
+    }
-
+
-    return false;
+    // assist if wind speed is below cutoff
+    return (is_positive(sail_windspeed_min) &&
+            rover.g2.windvane.wind_speed_enabled() &&
+            (rover.g2.windvane.get_true_wind_speed() < sail_windspeed_min));
 }

APMrover2/sailboat.h

@@ -66,20 +66,27 @@ public:
     // calculate the heading to sail on if we cant go upwind
     float calc_heading(float desired_heading_cd);
 
-    // throttle state used with throttle enum
+    // states of USE_MOTOR parameter and motor_state variable
-    enum Sailboat_Throttle {
+    enum class UseMotor {
-        NEVER        = 0,
+        USE_MOTOR_NEVER  = 0,
-        ASSIST       = 1,
+        USE_MOTOR_ASSIST = 1,
-        FORCE_MOTOR  = 2
+        USE_MOTOR_ALWAYS = 2
-    } throttle_state;
+    };
+
+    // 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);
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
 
-    // Check if we should assist with throttle
+    // true if motor is on to assist with slow tack
-    bool throttle_assist() const;
+    bool motor_assist_tack() const;
+
+    // true if motor should be on to assist with low wind
+    bool motor_assist_low_wind() const;
 
     // parameters
     AP_Int8 enable;
@@ -88,7 +95,7 @@ private:
     AP_Float sail_angle_ideal;
     AP_Float sail_heel_angle_max;
     AP_Float sail_no_go;
-    AP_Float sail_assist_windspeed;
+    AP_Float sail_windspeed_min;
 
     enum Sailboat_Tack {
         TACK_PORT,
@@ -101,4 +108,5 @@ private:
     uint32_t auto_tack_request_ms;  // system time user requested tack in autonomous modes
     uint32_t auto_tack_start_ms;    // system time when tack was started in autonomous mode
     bool tack_assist;               // true if we should use some throttle to assist tack
+    UseMotor motor_state;           // current state of motor output
 };