Rover: make sailboat a class

APMrover2/Rover.h

@@ -93,6 +93,7 @@
 #include "AP_MotorsUGV.h"
 #include "mode.h"
 #include "AP_Arming.h"
+#include "sailboat.h"
 // Configuration
 #include "config.h"
 #include "defines.h"
@@ -132,6 +133,8 @@ public:
     friend class RC_Channel_Rover;
     friend class RC_Channels_Rover;
 
+    friend class Sailboat;
+
     Rover(void);
 
     // HAL::Callbacks implementation.
@@ -347,18 +350,6 @@ private:
     } cruise_learn_t;
     cruise_learn_t cruise_learn;
 
-    // sailboat variables
-    enum Sailboat_Tack {
-        TACK_PORT,
-        TACK_STARBOARD
-    };
-    struct {
-        bool 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
-    } sailboat;
-
 private:
 
     // APMrover2.cpp
@@ -444,17 +435,6 @@ private:
     void radio_failsafe_check(uint16_t pwm);
     bool trim_radio();
 
-    // sailboat.cpp
-    void sailboat_update_mainsail(float desired_speed);
-    float sailboat_get_VMG() const;
-    void sailboat_handle_tack_request_acro();
-    float sailboat_get_tack_heading_rad() const;
-    void sailboat_handle_tack_request_auto();
-    void sailboat_clear_tack();
-    bool sailboat_tacking() const;
-    bool sailboat_use_indirect_route(float desired_heading_cd) const;
-    float sailboat_calc_heading(float desired_heading_cd);
-
     // sensors.cpp
     void init_compass_location(void);
     void update_compass(void);
@@ -525,6 +505,9 @@ public:
 
     // Simple mode
     float simple_sin_yaw;
+
+    // sailboat enabled
+    bool get_sailboat_enable() { return g2.sailboat.enabled(); }
 };
 
 extern const AP_HAL::HAL& hal;

APMrover2/sailboat.cpp

@@ -20,95 +20,174 @@ To Do List
  - add option to do proper tacks, ie tacking on flat spot in the waves, or only try once at a certain speed, or some better method than just changing the desired heading suddenly
 */
 
+const AP_Param::GroupInfo Sailboat::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Enable Sailboat
+    // @Description: This enables Sailboat functionality
+    // @Values: 0:Disable,1:Enable
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO_FLAGS("ENABLE", 1, Sailboat, enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: ANGLE_MIN
+    // @DisplayName: Sail min angle
+    // @Description: Mainsheet tight, angle between centerline and boom
+    // @Units: deg
+    // @Range: 0 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("ANGLE_MIN", 2, Sailboat, sail_angle_min, 0),
+
+    // @Param: ANGLE_MAX
+    // @DisplayName: Sail max angle
+    // @Description: Mainsheet loose, angle between centerline and boom
+    // @Units: deg
+    // @Range: 0 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("ANGLE_MAX", 3, Sailboat, sail_angle_max, 90),
+
+    // @Param: ANGLE_IDEAL
+    // @DisplayName: Sail ideal angle
+    // @Description: Ideal angle between sail and apparent wind
+    // @Units: deg
+    // @Range: 0 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("ANGLE_IDEAL", 4, Sailboat, sail_angle_ideal, 25),
+
+    // @Param: HEEL_MAX
+    // @DisplayName: Sailing maximum heel angle
+    // @Description: When in auto sail trim modes the heel will be limited to this value using PID control
+    // @Units: deg
+    // @Range: 0 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("HEEL_MAX", 5, Sailboat, sail_heel_angle_max, 15),
+
+    // @Param: SAIL_NO_GO_ANGLE
+    // @DisplayName: Sailing no go zone angle
+    // @Description: The typical closest angle to the wind the vehicle will sail at. the vehicle will sail at this angle when going upwind
+    // @Units: deg
+    // @Range: 0 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("NO_GO_ANGLE", 6, Sailboat, sail_no_go, 45),
+
+    AP_GROUPEND
+};
+
+
+/*
+  constructor
+ */
+Sailboat::Sailboat()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void Sailboat::init()
+{
+    // sailboat defaults
+    if (enabled()) {
+        rover.g2.crash_angle.set_default(0);
+        rover.g2.loit_type.set_default(1);
+        rover.g2.loit_radius.set_default(5);
+        rover.g2.wp_nav.set_default_overshoot(10);
+    }
+}
+
 // update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
-void Rover::sailboat_update_mainsail(float desired_speed)
+void Sailboat::update_mainsail(float desired_speed)
 {
-    if (!g2.motors.has_sail()) {
+    if (!enabled()) {
         return;
     }
 
     // relax sail if desired speed is zero
     if (!is_positive(desired_speed)) {
-        g2.motors.set_mainsail(100.0f);
+        rover.g2.motors.set_mainsail(100.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(g2.windvane.get_apparent_wind_direction_rad());
+    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 - g2.sail_angle_ideal;
+    float mainsail_angle = wind_dir_apparent -sail_angle_ideal;
 
     // make sure between allowable range
-    mainsail_angle = constrain_float(mainsail_angle, g2.sail_angle_min, g2.sail_angle_max);
+    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, g2.sail_angle_min, g2.sail_angle_max);
+    float mainsail = linear_interpolate(0.0f, 100.0f, mainsail_angle,sail_angle_min,sail_angle_max);
 
     // use PID controller to sheet out
-    const float pid_offset = g2.attitude_control.get_sail_out_from_heel(radians(g2.sail_heel_angle_max), 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;
 
     mainsail = constrain_float((mainsail+pid_offset), 0.0f ,100.0f);
-    g2.motors.set_mainsail(mainsail);
+    rover.g2.motors.set_mainsail(mainsail);
 }
 
 // Velocity Made Good, this is the speed we are traveling towards the desired destination
 // only for logging at this stage
 // https://en.wikipedia.org/wiki/Velocity_made_good
-float Rover::sailboat_get_VMG() const
+float Sailboat::get_VMG() const
 {
     // return 0 if not heading towards waypoint
-    if (!control_mode->is_autopilot_mode()) {
+    if (!rover.control_mode->is_autopilot_mode()) {
         return 0.0f;
     }
 
     float speed;
-    if (!g2.attitude_control.get_forward_speed(speed)) {
+    if (!rover.g2.attitude_control.get_forward_speed(speed)) {
         return 0.0f;
     }
-    return (speed * cosf(wrap_PI(radians(g2.wp_nav.wp_bearing_cd() * 0.01f) - ahrs.yaw)));
+
+    return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.yaw)));
 }
 
 // handle user initiated tack while in acro mode
-void Rover::sailboat_handle_tack_request_acro()
+void Sailboat::handle_tack_request_acro()
 {
     // set tacking heading target to the current angle relative to the true wind but on the new tack
-    sailboat.tacking = true;
-    sailboat.tack_heading_rad = wrap_2PI(ahrs.yaw + 2.0f * wrap_PI((g2.windvane.get_absolute_wind_direction_rad() - ahrs.yaw)));
+    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)));
 }
 
 // return target heading in radians when tacking (only used in acro)
-float Rover::sailboat_get_tack_heading_rad() const
+float Sailboat::get_tack_heading_rad() const
 {
-    return sailboat.tack_heading_rad;
+    return tack_heading_rad;
 }
 
 // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
-void Rover::sailboat_handle_tack_request_auto()
+void Sailboat::handle_tack_request_auto()
 {
     // record time of request for tack.  This will be processed asynchronously by sailboat_calc_heading
-    sailboat.auto_tack_request_ms = AP_HAL::millis();
+    auto_tack_request_ms = AP_HAL::millis();
 }
 
 // clear tacking state variables
-void Rover::sailboat_clear_tack()
+void Sailboat::clear_tack()
 {
-    sailboat.tacking = false;
-    sailboat.auto_tack_request_ms = 0;
+    currently_tacking = false;
+    auto_tack_request_ms = 0;
 }
 
 // returns true if boat is currently tacking
-bool Rover::sailboat_tacking() const
+bool Sailboat::tacking() const
 {
-    return sailboat.tacking;
+    return enabled() && currently_tacking;
 }
 
 // returns true if sailboat should take a indirect navigation route to go upwind
 // desired_heading should be in centi-degrees
-bool Rover::sailboat_use_indirect_route(float desired_heading_cd) const
+bool Sailboat::use_indirect_route(float desired_heading_cd) const
 {
-    if (!g2.motors.has_sail()) {
+    if (!enabled()) {
         return false;
     }
 
@@ -116,14 +195,14 @@ bool Rover::sailboat_use_indirect_route(float desired_heading_cd) const
     const float desired_heading_rad = radians(desired_heading_cd * 0.01f);
 
     // check if desired heading is in the no go zone, if it is we can't go direct
-    return fabsf(wrap_PI(g2.windvane.get_absolute_wind_direction_rad() - desired_heading_rad)) <= radians(g2.sail_no_go);
+    return fabsf(wrap_PI(rover.g2.windvane.get_absolute_wind_direction_rad() - desired_heading_rad)) <= radians(sail_no_go);
 }
 
 // if we can't sail on the desired heading then we should pick the best heading that we can sail on
 // this function assumes the caller has already checked sailboat_use_indirect_route(desired_heading_cd) returned true
-float Rover::sailboat_calc_heading(float desired_heading_cd)
+float Sailboat::calc_heading(float desired_heading_cd)
 {
-    if (!g2.motors.has_sail()) {
+    if (!enabled()) {
         return desired_heading_cd;
     }
 
@@ -131,19 +210,19 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
 
     // check for user requested tack
     uint32_t now = AP_HAL::millis();
-    if (sailboat.auto_tack_request_ms != 0) {
+    if (auto_tack_request_ms != 0) {
         // set should_tack flag is user requested tack within last 0.5 sec
-        should_tack = ((now - sailboat.auto_tack_request_ms) < 500);
-        sailboat.auto_tack_request_ms = 0;
+        should_tack = ((now - auto_tack_request_ms) < 500);
+        auto_tack_request_ms = 0;
     }
 
     // calculate left and right no go headings looking upwind
-    const float left_no_go_heading_rad = wrap_2PI(g2.windvane.get_absolute_wind_direction_rad() + radians(g2.sail_no_go));
-    const float right_no_go_heading_rad = wrap_2PI(g2.windvane.get_absolute_wind_direction_rad() - radians(g2.sail_no_go));
+    const float left_no_go_heading_rad = wrap_2PI(rover.g2.windvane.get_absolute_wind_direction_rad() + radians(sail_no_go));
+    const float right_no_go_heading_rad = wrap_2PI(rover.g2.windvane.get_absolute_wind_direction_rad() - radians(sail_no_go));
 
     // calculate current tack, Port if heading is left of no-go, STBD if right of no-go
     Sailboat_Tack current_tack;
-    if (is_negative(g2.windvane.get_apparent_wind_direction_rad())) {
+    if (is_negative(rover.g2.windvane.get_apparent_wind_direction_rad())) {
         current_tack = TACK_PORT;
     } else {
         current_tack = TACK_STARBOARD;
@@ -151,14 +230,15 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
 
     // trigger tack if cross track error larger than waypoint_overshoot parameter
     // this effectively defines a 'corridor' of width 2*waypoint_overshoot that the boat will stay within
-    if ((fabsf(g2.wp_nav.crosstrack_error()) >= g2.wp_nav.get_overshoot()) && !is_zero(g2.wp_nav.get_overshoot()) && !sailboat_tacking()) {
+    const float cross_track_error = rover.g2.wp_nav.crosstrack_error();
+    if ((fabsf(cross_track_error) >= rover.g2.wp_nav.get_overshoot()) && !is_zero(rover.g2.wp_nav.get_overshoot()) && !currently_tacking) {
         // make sure the new tack will reduce the cross track error
         // if were on starboard tack we are traveling towards the left hand boundary
-        if (is_positive(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
+        if (is_positive(cross_track_error) && (current_tack == TACK_STARBOARD)) {
             should_tack = true;
         }
         // if were on port tack we are traveling towards the right hand boundary
-        if (is_negative(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_PORT)) {
+        if (is_negative(cross_track_error) && (current_tack == TACK_PORT)) {
             should_tack = true;
         }
     }
@@ -169,28 +249,28 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
         // calculate target heading for the new tack
         switch (current_tack) {
             case TACK_PORT:
-                sailboat.tack_heading_rad = right_no_go_heading_rad;
+                tack_heading_rad = right_no_go_heading_rad;
                 break;
             case TACK_STARBOARD:
-                sailboat.tack_heading_rad = left_no_go_heading_rad;
+                tack_heading_rad = left_no_go_heading_rad;
                 break;
         }
-        sailboat.tacking = true;
-        sailboat.auto_tack_start_ms = AP_HAL::millis();
+        currently_tacking = true;
+        auto_tack_start_ms = AP_HAL::millis();
     }
 
     // if we are tacking we maintain the target heading until the tack completes or times out
-    if (sailboat.tacking) {
+    if (currently_tacking) {
         // check if we have reached target
-        if (fabsf(wrap_PI(sailboat.tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
-            sailboat_clear_tack();
-        } else if ((now - sailboat.auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
+        if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
+            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");
-            sailboat_clear_tack();
+            clear_tack();
         }
         // return tack target heading
-        return degrees(sailboat.tack_heading_rad) * 100.0f;
+        return degrees(tack_heading_rad) * 100.0f;
     }
 
     // return closest possible heading to wind

APMrover2/sailboat.h

@@ -0,0 +1,81 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+    Rover Sailboat functionality
+*/
+class Sailboat
+{
+public:
+
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+    // enabled
+    bool enabled() const { return enable != 0;}
+
+    // constructor
+    Sailboat();
+
+    // 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);
+
+    // 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() const;
+
+    // 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);
+
+private:
+
+    // parameters
+    AP_Int8 enable;
+    AP_Float sail_angle_min;
+    AP_Float sail_angle_max;
+    AP_Float sail_angle_ideal;
+    AP_Float sail_heel_angle_max;
+    AP_Float sail_no_go;
+
+    enum Sailboat_Tack {
+        TACK_PORT,
+        TACK_STARBOARD
+    };
+
+    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
+};

APMrover2/system.cpp

@@ -75,6 +75,8 @@ void Rover::init_ardupilot()
 
     g2.windvane.init();
 
+    rover.g2.sailboat.init();
+
     // init baro before we start the GCS, so that the CLI baro test works
     barometer.init();