Rover: add sailboat tacking

APMrover2/APMrover2.cpp

@@ -96,6 +96,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
 #if ADVANCED_FAILSAFE == ENABLED
     SCHED_TASK(afs_fs_check,           10,    200),
 #endif
+    SCHED_TASK(read_airspeed,          10,    100),
 };
 
 void Rover::read_mode_switch()

APMrover2/GCS_Mavlink.cpp

@@ -289,8 +289,8 @@ void Rover::send_wind(mavlink_channel_t chan)
     // send wind
     mavlink_msg_wind_send(
         chan,
-        degrees(rover.g2.windvane.get_apparent_wind_direction_rad()),
-        0,
+        wrap_360(degrees(g2.windvane.get_absolute_wind_direction_rad())),
+        g2.windvane.get_true_wind_speed(),
         0);
 }
 

APMrover2/Log.cpp

@@ -167,14 +167,23 @@ void Rover::Log_Write_Sail()
     }
 
     // get wind direction
+    float wind_dir_abs = 0.0f;
     float wind_dir_rel = 0.0f;
+    float wind_speed_true = 0.0f;
+    float wind_speed_apparent = 0.0f;
     if (rover.g2.windvane.enabled()) {
+        wind_dir_abs = degrees(g2.windvane.get_absolute_wind_direction_rad());
         wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad());
+        wind_speed_true = g2.windvane.get_true_wind_speed();
+        wind_speed_apparent = g2.windvane.get_apparent_wind_speed();
     }
-    DataFlash.Log_Write("SAIL", "TimeUS,WindDirRel,SailOut,VMG",
-                        "sh%n", "F000", "Qfff",
+    DataFlash.Log_Write("SAIL", "TimeUS,WindDirAbs,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG",
+                        "shhnn%n", "F000000", "Qffffff",
                         AP_HAL::micros64(),
+                        (double)wind_dir_abs,
                         (double)wind_dir_rel,
+                        (double)wind_speed_true,
+                        (double)wind_speed_apparent,
                         (double)g2.motors.get_mainsail(),
                         (double)sailboat_get_VMG());
 }

APMrover2/Parameters.cpp

@@ -647,6 +647,20 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Increment: 1
     // @User: Standard
     AP_GROUPINFO("SAIL_HEEL_MAX", 35, ParametersG2, 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("SAIL_NO_GO_ANGLE", 36, ParametersG2, sail_no_go, 45),
+
+    // @Group: ARSPD
+    // @Path: ../libraries/AP_WindVane/AP_WindVane.cpp
+    AP_SUBGROUPINFO(airspeed, "ARSPD", 37, ParametersG2, AP_Airspeed),
+
     AP_GROUPEND
 };
 
@@ -679,7 +693,8 @@ ParametersG2::ParametersG2(void)
     avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
     follow(),
     rally(rover.ahrs),
-    windvane()
+    windvane(),
+    airspeed()
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -377,9 +377,13 @@ public:
     AP_Float sail_angle_max;
     AP_Float sail_angle_ideal;
     AP_Float sail_heel_angle_max;
+    AP_Float sail_no_go;
 
     // windvane
     AP_WindVane windvane;
+
+    // Airspeed
+    AP_Airspeed airspeed;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/RC_Channel.cpp

@@ -90,6 +90,7 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const aux_s
     case GUIDED:
     case LOITER:
     case FOLLOW:
+    case SAILBOAT_TACK:
         break;
     default:
         RC_Channel::init_aux_function(ch_option, ch_flag);
@@ -234,6 +235,13 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
         do_aux_function_change_mode(rover.mode_simple, ch_flag);
         break;
 
+    // trigger sailboat tack
+    case SAILBOAT_TACK:
+        if (ch_flag == HIGH) {
+            rover.control_mode->handle_tack_request();
+        }
+        break;
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;

APMrover2/Rover.h

@@ -387,6 +387,16 @@ private:
         LowPassFilterFloat throttle_filt = LowPassFilterFloat(2.0f);
     } cruise_learn;
 
+    // sailboat variables
+    enum Sailboat_Tack {
+        Tack_Port,
+        Tack_STBD
+    };
+    bool _sailboat_tacking;                 // true when sailboat is in the process of tacking to a new heading
+    float _sailboat_tack_heading_rad;       // target heading in radians while tacking in either acro or autonomous modes
+    uint32_t _sailboat_auto_tack_request_ms;// system time user requested tack in autonomous modes
+    uint32_t _sailboat_auto_tack_start_ms;  // system time when tack was started in autonomous mode
+
 private:
 
     // APMrover2.cpp
@@ -508,6 +518,13 @@ private:
     // 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(void);
@@ -521,6 +538,7 @@ private:
     void accel_cal_update(void);
     void read_rangefinders(void);
     void init_proximity();
+    void read_airspeed();
     void update_sensor_status_flags(void);
 
     // Steering.cpp

APMrover2/mode.cpp

@@ -47,6 +47,9 @@ bool Mode::enter()
     // initialisation common to all modes
     if (ret) {
         set_reversed(false);
+
+        // clear sailboat tacking flags
+        rover.sailboat_clear_tack();
     }
 
     return ret;
@@ -254,6 +257,15 @@ void Mode::set_reversed(bool value)
     _reversed = value;
 }
 
+// handle tacking request (from auxiliary switch) in sailboats
+void Mode::handle_tack_request()
+{
+    // autopilot modes handle tacking
+    if (is_autopilot_mode()) {
+        rover.sailboat_handle_tack_request_auto();
+    }
+}
+
 void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
 {
     // add in speed nudging
@@ -435,7 +447,11 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct
     }
     _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
 
-    if (rover.use_pivot_steering(_yaw_error_cd)) {
+    if (rover.sailboat_use_indirect_route(desired_heading)) {
+        // sailboats use heading controller when tacking upwind
+        desired_heading = rover.sailboat_calc_heading(desired_heading);
+        calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
+    } else if (rover.use_pivot_steering(_yaw_error_cd)) {
         // for pivot turns use heading controller
         calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
     } else {

APMrover2/mode.h

@@ -116,6 +116,9 @@ public:
     // execute the mission in reverse (i.e. backing up)
     void set_reversed(bool value);
 
+    // handle tacking request (from auxiliary switch) in sailboats
+    virtual void handle_tack_request();
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -222,6 +225,9 @@ public:
     // acro mode requires a velocity estimate for non skid-steer rovers
     bool requires_position() const override { return false; }
     bool requires_velocity() const override;
+
+    // sailboats in acro mode support user manually initiating tacking from transmitter
+    void handle_tack_request() override;
 };
 
 

APMrover2/mode_acro.cpp

@@ -18,14 +18,30 @@ void ModeAcro::update()
         calc_throttle(desired_speed, false, true);
     }
 
-    // convert pilot steering input to desired turn rate in radians/sec
-    const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
+    float steering_out;
 
-    // run steering turn rate controller and throttle controller
-    const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
-                                                                      g2.motors.limit.steer_left,
-                                                                      g2.motors.limit.steer_right,
-                                                                      rover.G_Dt);
+    // handle sailboats
+    if (!is_zero(desired_steering)) {
+        // steering input return control to user
+        rover.sailboat_clear_tack();
+    }
+    if (g2.motors.has_sail() && rover.sailboat_tacking()) {
+        // call heading controller during tacking
+        steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
+                                                                 g2.pivot_turn_rate,
+                                                                 g2.motors.limit.steer_left,
+                                                                 g2.motors.limit.steer_right,
+                                                                 rover.G_Dt);
+    } else {
+        // convert pilot steering input to desired turn rate in radians/sec
+        const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
+
+        // run steering turn rate controller and throttle controller
+        steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
+                                                              g2.motors.limit.steer_left,
+                                                              g2.motors.limit.steer_right,
+                                                              rover.G_Dt);
+    }
 
     g2.motors.set_steering(steering_out * 4500.0f);
 }
@@ -34,3 +50,9 @@ bool ModeAcro::requires_velocity() const
 {
     return g2.motors.have_skid_steering()? false: true;
 }
+
+// sailboats in acro mode support user manually initiating tacking from transmitter
+void ModeAcro::handle_tack_request()
+{
+    rover.sailboat_handle_tack_request_acro();
+}

APMrover2/sailboat.cpp

@@ -1,5 +1,7 @@
 #include "Rover.h"
 
+#define SAILBOAT_AUTO_TACKING_TIMEOUT_MS 50000  // tacks in auto mode timeout if not successfully completed within this many milliseconds
+#define SAILBOAT_TACKING_ACCURACY_DEG 10        // tack is considered complete when vehicle is within this many degrees of target tack angle
 /*
 To Do List
  - Improve tacking in light winds and bearing away in strong wings
@@ -67,3 +69,131 @@ float Rover::sailboat_get_VMG() const
     }
     return (speed * cosf(wrap_PI(radians(nav_controller->target_bearing_cd()) - ahrs.yaw)));
 }
+
+// handle user initiated tack while in acro mode
+void Rover::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)));
+}
+
+// return target heading in radians when tacking (only used in acro)
+float Rover::sailboat_get_tack_heading_rad() const
+{
+    return _sailboat_tack_heading_rad;
+}
+
+// handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL)
+void Rover::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();
+}
+
+// clear tacking state variables
+void Rover::sailboat_clear_tack()
+{
+    _sailboat_tacking = false;
+    _sailboat_auto_tack_request_ms = 0;
+}
+
+// returns true if boat is currently tacking
+bool Rover::sailboat_tacking() const
+{
+    return _sailboat_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
+{
+    if (!g2.motors.has_sail()) {
+        return false;
+    }
+
+    // convert desired heading to radians
+    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);
+}
+
+// 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)
+{
+    if (!g2.motors.has_sail()) {
+        return desired_heading_cd;
+    }
+
+    bool should_tack = false;
+
+    // check for user requested tack
+    uint32_t now = AP_HAL::millis();
+    if (_sailboat_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;
+    }
+
+    // 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));
+
+    // 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())) {
+        current_tack = Tack_Port;
+    } else {
+        current_tack = Tack_STBD;
+    }
+
+    // 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(rover.nav_controller->crosstrack_error()) >= g.waypoint_overshoot) && !is_zero(g.waypoint_overshoot) && !sailboat_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(rover.nav_controller->crosstrack_error()) && (current_tack == Tack_STBD)) {
+            should_tack = true;
+        }
+        // if were on port tack we are traveling towards the right hand boundary
+        if (is_negative(rover.nav_controller->crosstrack_error()) && (current_tack == Tack_Port)) {
+            should_tack = true;
+        }
+    }
+
+    // if tack triggered, calculate target heading
+    if (should_tack) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking");
+        // calculate target heading for the new tack
+        switch (current_tack) {
+            case Tack_Port:
+                _sailboat_tack_heading_rad = right_no_go_heading_rad;
+                break;
+            case Tack_STBD:
+                _sailboat_tack_heading_rad = left_no_go_heading_rad;
+                break;
+        }
+        _sailboat_tacking = true;
+        _sailboat_auto_tack_start_ms = AP_HAL::millis();
+    }
+
+    // if were are tacking we maintain the target heading until the tack completes or timesout
+    if (_sailboat_tacking) {
+        // if we have reached target heading or timed out stop tacking on the next iteration
+        if (((now - _sailboat_auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) ||
+            (fabsf(wrap_PI(_sailboat_tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG))) {
+            _sailboat_tacking = false;
+        }
+        // return tack target heading
+        return degrees(_sailboat_tack_heading_rad) * 100.0f;
+    }
+
+    // return closest possible heading to wind
+    if (current_tack == Tack_Port) {
+        return degrees(left_no_go_heading_rad) * 100.0f;
+    } else {
+        return degrees(right_no_go_heading_rad) * 100.0f;
+    }
+}

APMrover2/sensors.cpp

@@ -241,6 +241,26 @@ void Rover::init_proximity(void)
     g2.proximity.set_rangefinder(&rangefinder);
 }
 
+/*
+  ask airspeed sensor for a new value, duplicated from plane
+ */
+void Rover::read_airspeed(void)
+{
+    if (g2.airspeed.enabled()) {
+        g2.airspeed.read();
+        if (should_log(MASK_LOG_IMU)) {
+            DataFlash.Log_Write_Airspeed(g2.airspeed);
+        }
+
+        // supply a new temperature to the barometer from the digital
+        // airspeed sensor if we can
+        float temperature;
+        if (g2.airspeed.get_temperature(temperature)) {
+            barometer.set_external_temperature(temperature);
+        }
+    }
+}
+
 // update error mask of sensors and subsystems. The mask
 // uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
 // then it indicates that the sensor or subsystem is present but

APMrover2/system.cpp

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