Rover: sailboat add motor-sailing

APMrover2/Log.cpp

@@ -28,7 +28,7 @@ void Rover::Log_Write_Attitude()
     }
 
     // log heel to sail control for sailboats
-    if (rover.g2.sailboat.enabled()) {
+    if (rover.g2.sailboat.sail_enabled()) {
         logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
     }
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -123,7 +123,7 @@ void Rover::Log_Write_Nav_Tuning()
 void Rover::Log_Write_Sail()
 {
     // only log sail if present
-    if (!rover.g2.sailboat.enabled()) {
+    if (!rover.g2.sailboat.sail_enabled()) {
         return;
     }
 

APMrover2/Rover.h

@@ -486,9 +486,6 @@ public:
 
     // Simple mode
     float simple_sin_yaw;
-
-    // sailboat enabled
-    bool get_sailboat_enable() { return g2.sailboat.enabled(); }
 };
 
 extern const AP_HAL::HAL& hal;

APMrover2/mode.cpp

@@ -272,24 +272,28 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
     }
 
     // call throttle controller and convert output to -100 to +100 range
-    float throttle_out;
+    float throttle_out = 0.0f;
 
-    // call speed or stop controller
-    if (is_zero(target_speed) && !rover.is_balancebot()) {
-        bool stopped;
-        throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
+    if (rover.g2.sailboat.sail_enabled()) {
+        // sailboats use special throttle and mainsail controller
+        float mainsail_out = 0.0f;
+        rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out);
+        rover.g2.motors.set_mainsail(mainsail_out);
     } else {
-        throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
-    }
+        // call speed or stop controller
+        if (is_zero(target_speed) && !rover.is_balancebot()) {
+            bool stopped;
+            throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
+        } else {
+            throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
+        }
 
-    // if vehicle is balance bot, calculate actual throttle required for balancing
-    if (rover.is_balancebot()) {
-        rover.balancebot_pitch_control(throttle_out);
+        // if vehicle is balance bot, calculate actual throttle required for balancing
+        if (rover.is_balancebot()) {
+            rover.balancebot_pitch_control(throttle_out);
+        }
     }
 
-    // update mainsail position if present
-    rover.g2.sailboat.update_mainsail(target_speed);
-
     // send to motor
     g2.motors.set_throttle(throttle_out);
 }

APMrover2/mode_loiter.cpp

@@ -24,8 +24,8 @@ void ModeLoiter::update()
 
     // if within loiter radius slew desired speed towards zero and use existing desired heading
     if (_distance_to_destination <= g2.loit_radius) {
-        // sailboats do not stop
-        const float desired_speed_within_radius = rover.g2.sailboat.enabled() ? 0.1f : 0.0f;
+        // sailboats should not stop unless motoring
+        const float desired_speed_within_radius = rover.g2.sailboat.nav_enabled() ? 0.1f : 0.0f;
         _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
     } else {
         // P controller with hard-coded gain to convert distance to desired speed

APMrover2/mode_manual.cpp

@@ -18,8 +18,10 @@ void ModeManual::update()
         rover.balancebot_pitch_control(desired_throttle);
     }
 
-    // set sailboat mainsail from throttle position
-    g2.motors.set_mainsail(desired_throttle);
+    // set sailboat mainsail
+    float desired_mainsail;
+    g2.sailboat.get_pilot_desired_mainsail(desired_mainsail);
+    g2.motors.set_mainsail(desired_mainsail);
 
     // copy RC scaled inputs to outputs
     g2.motors.set_throttle(desired_throttle);

APMrover2/radio.cpp

@@ -17,6 +17,9 @@ void Rover::set_control_channels(void)
         channel_lateral->set_angle(100);
     }
 
+    // sailboat rc input init
+    g2.sailboat.init_rc_in();
+
     // Allow to reconfigure output when not armed
     if (!arming.is_armed()) {
         g2.motors.setup_servo_output();

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
+    // @Values: 0:Disable,1:Enable sail assist only,2:Enable
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("ENABLE", 1, Sailboat, enable, 0, AP_PARAM_FLAG_ENABLE),
@@ -75,6 +75,15 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("NO_GO_ANGLE", 6, Sailboat, sail_no_go, 45),
 
+    // @Param: WNDSPD_MIN
+    // @DisplayName: Sailboat minimum wind speed to sail in
+    // @Description: Sailboat minimum wind speed to continue sail in, at lower wind speeds the sailboat will motor if one is fitted
+    // @Units: m/s
+    // @Range: 0 5
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_assist_windspeed, 0),
+
     AP_GROUPEND
 };
 
@@ -87,48 +96,111 @@ Sailboat::Sailboat()
     AP_Param::setup_object_defaults(this, var_info);
 }
 
+// Should we use sailboat navigation?
+bool Sailboat::nav_enabled() const
+{
+    return (enable == 2) &&
+           (throttle_state != Sailboat_Throttle::FORCE_MOTOR) &&
+           (!throttle_assist() || tack_assist);
+}
+
 void Sailboat::init()
 {
     // sailboat defaults
-    if (enabled()) {
+    if (sail_enabled()) {
         rover.g2.crash_angle.set_default(0);
+    }
+
+    if (nav_enabled()) {
         rover.g2.loit_type.set_default(1);
         rover.g2.loit_radius.set_default(5);
         rover.g2.wp_nav.set_default_overshoot(10);
     }
+
+    // if we have a throttle of some sort allow to use it
+    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) {
+        throttle_state = Sailboat_Throttle::ASSIST;
+    }
+}
+
+// initialise rc input (channel_mainsail), may be called intermittently
+void Sailboat::init_rc_in()
+{
+    // get auxiliary throttle value
+    RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MAINSAIL);
+    if (rc_ptr != nullptr) {
+        // use aux as sail input if defined
+        channel_mainsail = rc_ptr;
+        channel_mainsail->set_angle(100);
+        channel_mainsail->set_default_dead_zone(30);
+    } else {
+        // use throttle channel
+        channel_mainsail = rover.channel_throttle;
+    }
 }
 
-// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
-void Sailboat::update_mainsail(float desired_speed)
+// 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)
 {
-    if (!enabled()) {
+    // no RC input means mainsail is moved to trim
+    if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) {
+        mainsail_out = 100.0f;
         return;
     }
+    mainsail_out = constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f);
+}
 
-    // relax sail if desired speed is zero
-    if (!is_positive(desired_speed)) {
-        rover.g2.motors.set_mainsail(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)
+{
+    if (!sail_enabled()) {
+        throttle_out = 0.0f;
+        mainsail_out = 0.0f;
         return;
     }
 
-    // + 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
-    float wind_dir_apparent = fabsf(rover.g2.windvane.get_apparent_wind_direction_rad());
-    wind_dir_apparent = degrees(wind_dir_apparent);
+    // run speed controller if motor is forced on or if assistance is enabled for low speeds or tacking
+    if ((throttle_state == Sailboat_Throttle::FORCE_MOTOR) || throttle_assist()) {
+        // 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;
+    }
+
+    //
+    // mainsail control
+    //
+    // if we are motoring or attempting to reverse relax the sail
+    if (throttle_state == Sailboat_Throttle::FORCE_MOTOR || !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
+        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;
+        // set the main sail to the ideal angle to the wind
+        float mainsail_angle = wind_dir_apparent -sail_angle_ideal;
 
-    // make sure between allowable range
-    mainsail_angle = constrain_float(mainsail_angle,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 = linear_interpolate(0.0f, 100.0f, 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);
 
-    // use PID controller to sheet out
-    const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
+        // use PID controller to sheet out
+        const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
 
-    mainsail = constrain_float((mainsail+pid_offset), 0.0f ,100.0f);
-    rover.g2.motors.set_mainsail(mainsail);
+        mainsail_out = constrain_float((mainsail_base + pid_offset), 0.0f ,100.0f);
+    }
 }
 
 // Velocity Made Good, this is the speed we are traveling towards the desired destination
@@ -152,9 +224,14 @@ float Sailboat::get_VMG() const
 // handle user initiated tack while in acro mode
 void Sailboat::handle_tack_request_acro()
 {
+    if (!nav_enabled() || currently_tacking) {
+        return;
+    }
     // set tacking heading target to the current angle relative to the true wind but on the new tack
     currently_tacking = true;
     tack_heading_rad = wrap_2PI(rover.ahrs.yaw + 2.0f * wrap_PI((rover.g2.windvane.get_absolute_wind_direction_rad() - rover.ahrs.yaw)));
+
+    auto_tack_request_ms = AP_HAL::millis();
 }
 
 // return target heading in radians when tacking (only used in acro)
@@ -166,6 +243,10 @@ float Sailboat::get_tack_heading_rad() const
 // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
 void Sailboat::handle_tack_request_auto()
 {
+    if (!nav_enabled() || currently_tacking) {
+        return;
+    }
+
     // record time of request for tack.  This will be processed asynchronously by sailboat_calc_heading
     auto_tack_request_ms = AP_HAL::millis();
 }
@@ -174,20 +255,21 @@ void Sailboat::handle_tack_request_auto()
 void Sailboat::clear_tack()
 {
     currently_tacking = false;
+    tack_assist = false;
     auto_tack_request_ms = 0;
 }
 
 // returns true if boat is currently tacking
 bool Sailboat::tacking() const
 {
-    return enabled() && currently_tacking;
+    return nav_enabled() && currently_tacking;
 }
 
 // returns true if sailboat should take a indirect navigation route to go upwind
 // desired_heading should be in centi-degrees
 bool Sailboat::use_indirect_route(float desired_heading_cd) const
 {
-    if (!enabled()) {
+    if (!nav_enabled()) {
         return false;
     }
 
@@ -202,7 +284,7 @@ bool Sailboat::use_indirect_route(float desired_heading_cd) const
 // this function assumes the caller has already checked sailboat_use_indirect_route(desired_heading_cd) returned true
 float Sailboat::calc_heading(float desired_heading_cd)
 {
-    if (!enabled()) {
+    if (!nav_enabled()) {
         return desired_heading_cd;
     }
 
@@ -266,8 +348,13 @@ 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
-            gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking timed out");
-            clear_tack();
+            if ((throttle_state == Sailboat_Throttle::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 {
+                gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking timed out");
+                clear_tack();
+            }
         }
         // return tack target heading
         return degrees(tack_heading_rad) * 100.0f;
@@ -280,3 +367,26 @@ float Sailboat::calc_heading(float desired_heading_cd)
         return degrees(right_no_go_heading_rad) * 100.0f;
     }
 }
+
+// should we use the throttle?
+bool Sailboat::throttle_assist() const
+{
+    // throttle is assist is disabled
+    if (throttle_state != Sailboat_Throttle::ASSIST) {
+        return false;
+    }
+
+    // assist with a tack
+    if (tack_assist) {
+        return true;
+    }
+
+    // wind speed is less than sailing cut-off
+    if (!is_zero(sail_assist_windspeed) &&
+        rover.g2.windvane.wind_speed_enabled() &&
+        rover.g2.windvane.get_true_wind_speed() < sail_assist_windspeed) {
+        return true;
+    }
+
+    return false;
+}

APMrover2/sailboat.h

@@ -24,7 +24,10 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
     // enabled
-    bool enabled() const { return enable != 0;}
+    bool sail_enabled() const { return enable > 0;}
+
+    // true if sailboat navigation (aka tacking) is enabled
+    bool nav_enabled() const;
 
     // constructor
     Sailboat();
@@ -32,8 +35,15 @@ public:
     // setup
     void init();
 
-    // update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
-    void  update_mainsail(float desired_speed);
+    // 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
+    void get_pilot_desired_mainsail(float &mainsail_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);
 
     // Velocity Made Good, this is the speed we are traveling towards the desired destination
     float get_VMG() const;
@@ -59,8 +69,18 @@ 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
+    enum Sailboat_Throttle {
+        NEVER        = 0,
+        ASSIST       = 1,
+        FORCE_MOTOR  = 2
+    } throttle_state;
+
 private:
 
+    // Check if we should assist with throttle
+    bool throttle_assist() const;
+
     // parameters
     AP_Int8 enable;
     AP_Float sail_angle_min;
@@ -68,14 +88,17 @@ private:
     AP_Float sail_angle_ideal;
     AP_Float sail_heel_angle_max;
     AP_Float sail_no_go;
+    AP_Float sail_assist_windspeed;
 
     enum Sailboat_Tack {
         TACK_PORT,
         TACK_STARBOARD
     };
 
+    RC_Channel *channel_mainsail;   // rc input channel for controlling mainsail
     bool currently_tacking;         // true when sailboat is in the process of tacking to a new heading
     float tack_heading_rad;         // target heading in radians while tacking in either acro or autonomous modes
     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
 };

APMrover2/system.cpp

@@ -75,8 +75,6 @@ void Rover::init_ardupilot()
 
     g2.windvane.init(serial_manager);
 
-    rover.g2.sailboat.init();
-
     // init baro before we start the GCS, so that the CLI baro test works
     barometer.init();
 
@@ -155,6 +153,8 @@ void Rover::init_ardupilot()
     // initialise rc channels
     rc().init();
 
+    rover.g2.sailboat.init();
+
     // disable safety if requested
     BoardConfig.init_safety();