Rover: add sailboat support

APMrover2/APMrover2.cpp

@@ -95,6 +95,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
 #if ADVANCED_FAILSAFE == ENABLED
     SCHED_TASK(afs_fs_check,           10,    200),
 #endif
+    SCHED_TASK(windvane_update,        10,    200),
 };
 
 void Rover::read_mode_switch()
@@ -181,6 +182,7 @@ void Rover::ahrs_update()
 
     if (should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
+        Log_Write_Sail();
     }
 
     if (should_log(MASK_LOG_IMU)) {
@@ -219,6 +221,7 @@ void Rover::update_logging1(void)
 {
     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
+        Log_Write_Sail();
     }
 
     if (should_log(MASK_LOG_THR)) {

APMrover2/GCS_Mavlink.cpp

@@ -257,6 +257,21 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
             return;
         }
     }
+
+    // sailboat heel to mainsail pid
+    if (g.gcs_pid_mask & 32) {
+        pid_info = &g2.attitude_control.get_sailboat_heel_pid().get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, 9,
+                                    pid_info->desired,
+                                    pid_info->actual,
+                                    pid_info->FF,
+                                    pid_info->P,
+                                    pid_info->I,
+                                    pid_info->D);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
 }
 
 void Rover::send_fence_status(mavlink_channel_t chan)
@@ -264,6 +279,21 @@ void Rover::send_fence_status(mavlink_channel_t chan)
     fence_send_mavlink_status(chan);
 }
 
+void Rover::send_wind(mavlink_channel_t chan)
+{
+    // exit immediately if no wind vane
+    if (!rover.g2.windvane.enabled()) {
+        return;
+    }
+
+    // send wind
+    mavlink_msg_wind_send(
+        chan,
+        degrees(rover.g2.windvane.get_apparent_wind_direction_rad()),
+        0,
+        0);
+}
+
 void Rover::send_wheel_encoder(mavlink_channel_t chan)
 {
     // send wheel encoder data using rpm message
@@ -347,6 +377,9 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
         rover.send_fence_status(chan);
         break;
 
+    case MSG_WIND:
+        CHECK_PAYLOAD_SIZE(WIND);
+        rover.send_wind(chan);
         break;
 
     case MSG_PID_TUNING:
@@ -492,6 +525,7 @@ static const ap_message STREAM_EXTRA2_msgs[] = {
 static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_AHRS,
     MSG_HWSTATUS,
+    MSG_WIND,
     MSG_RANGEFINDER,
     MSG_SYSTEM_TIME,
     MSG_BATTERY2,

APMrover2/Log.cpp

@@ -44,6 +44,11 @@ void Rover::Log_Write_Attitude()
     if (is_balancebot()) {
         DataFlash.Log_Write_PID(LOG_PIDP_MSG, g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info());
     }
+
+    // log heel to sail control for sailboats
+    if (g2.motors.has_sail()) {
+        DataFlash.Log_Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
+    }
 }
 
 // Write a range finder depth message
@@ -56,7 +61,7 @@ void Rover::Log_Write_Depth()
 
     // get position
     Location loc;
-    if (!rover.ahrs.get_position(loc)) {
+    if (!ahrs.get_position(loc)) {
         return;
     }
 
@@ -154,6 +159,26 @@ void Rover::Log_Write_Proximity()
     DataFlash.Log_Write_Proximity(g2.proximity);
 }
 
+void Rover::Log_Write_Sail()
+{
+    // only log sail if present
+    if (!g2.motors.has_sail()) {
+        return;
+    }
+
+    // get wind direction
+    float wind_dir_rel = 0.0f;
+    if (rover.g2.windvane.enabled()) {
+        wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad());
+    }
+    DataFlash.Log_Write("SAIL", "TimeUS,WindDirRel,SailOut,VMG",
+                        "sh%n", "F000", "Qfff",
+                        AP_HAL::micros64(),
+                        (double)wind_dir_rel,
+                        (double)g2.motors.get_mainsail(),
+                        (double)sailboat_get_VMG());
+}
+
 struct PACKED log_Steering {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -356,6 +381,7 @@ void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
 void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
 void Rover::Log_Write_Nav_Tuning() {}
 void Rover::Log_Write_Proximity() {}
+void Rover::Log_Write_Sail() {}
 void Rover::Log_Write_Startup(uint8_t type) {}
 void Rover::Log_Write_Throttle() {}
 void Rover::Log_Write_Rangefinder() {}

APMrover2/Parameters.cpp

@@ -64,8 +64,8 @@ const AP_Param::Info Rover::var_info[] = {
     // @DisplayName: GCS PID tuning mask
     // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
     // @User: Advanced
-    // @Values: 0:None,1:Steering,2:Throttle,4:Pitch,8:Left Wheel,16:Right Wheel
-    // @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel
+    // @Values: 0:None,1:Steering,2:Throttle,4:Pitch,8:Left Wheel,16:Right Wheel,32:Sailboat Heel
+    // @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel,5:Sailboat Heel
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
     // @Param: MAG_ENABLE
@@ -608,6 +608,45 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("LOIT_RADIUS", 30, ParametersG2, loit_radius, 2),
 
+    // @Group: WNDVN_
+    // @Path: ../libraries/AP_WindVane/AP_WindVane.cpp
+    AP_SUBGROUPINFO(windvane, "WNDVN_", 31, ParametersG2, AP_WindVane),
+
+    // @Param: SAIL_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("SAIL_ANGLE_MIN", 32, ParametersG2, sail_angle_min, 0),
+
+    // @Param: SAIL_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("SAIL_ANGLE_MAX", 33, ParametersG2, sail_angle_max, 90),
+
+    // @Param: SAIL_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("SAIL_ANGLE_IDEAL", 34, ParametersG2, sail_angle_ideal, 25),
+
+    // @Param: SAIL_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("SAIL_HEEL_MAX", 35, ParametersG2, sail_heel_angle_max, 15),
     AP_GROUPEND
 };
 
@@ -639,7 +678,8 @@ ParametersG2::ParametersG2(void)
     proximity(rover.serial_manager),
     avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
     follow(),
-    rally(rover.ahrs)
+    rally(rover.ahrs),
+    windvane()
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -704,6 +744,14 @@ void Rover::load_parameters(void)
         g2.crash_angle.set_default(30);
     }
 
+    // sailboat defaults
+    if (g2.motors.has_sail()) {
+        g2.crash_angle.set_default(0);
+        g2.loit_type.set_default(1);
+        g2.loit_radius.set_default(5);
+        g.waypoint_overshoot.set_default(10);
+    }
+
     const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
                                       Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,
                                       Parameters::k_param_rc_5_old,  Parameters::k_param_rc_6_old,

APMrover2/Parameters.h

@@ -371,6 +371,15 @@ public:
 
     // Simple mode types
     AP_Int8 simple_type;
+
+    // sailboat parameters
+    AP_Float sail_angle_min;
+    AP_Float sail_angle_max;
+    AP_Float sail_angle_ideal;
+    AP_Float sail_heel_angle_max;
+
+    // windvane
+    AP_WindVane windvane;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -79,6 +79,7 @@
 #include <AC_Avoidance/AC_Avoid.h>
 #include <AP_Follow/AP_Follow.h>
 #include <AP_OSD/AP_OSD.h>
+#include <AP_WindVane/AP_WindVane.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <SITL/SITL.h>
 #endif
@@ -472,6 +473,7 @@ private:
     void send_pid_tuning(mavlink_channel_t chan);
     void send_wheel_encoder(mavlink_channel_t chan);
     void send_fence_status(mavlink_channel_t chan);
+    void send_wind(mavlink_channel_t chan);
     void gcs_data_stream_send(void);
     void gcs_update(void);
     void gcs_retry_deferred(void);
@@ -484,6 +486,7 @@ private:
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
     void Log_Write_Nav_Tuning();
     void Log_Write_Proximity();
+    void Log_Write_Sail();
     void Log_Write_Startup(uint8_t type);
     void Log_Write_Steering();
     void Log_Write_Throttle();
@@ -506,6 +509,10 @@ private:
     void control_failsafe(uint16_t pwm);
     bool trim_radio();
 
+    // sailboat.cpp
+    void sailboat_update_mainsail(float desired_speed);
+    float sailboat_get_VMG() const;
+
     // sensors.cpp
     void init_compass(void);
     void init_compass_location(void);
@@ -518,6 +525,7 @@ private:
     void accel_cal_update(void);
     void read_rangefinders(void);
     void init_proximity();
+    void windvane_update();
     void update_sensor_status_flags(void);
 
     // Steering.cpp

APMrover2/mode.cpp

@@ -285,6 +285,9 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_
         rover.balancebot_pitch_control(throttle_out);
     }
 
+    // update mainsail position if present
+    rover.sailboat_update_mainsail(target_speed);
+
     // send to motor
     g2.motors.set_throttle(throttle_out);
 }
@@ -301,6 +304,9 @@ bool Mode::stop_vehicle()
         rover.balancebot_pitch_control(throttle_out);
     }
 
+    // relax mainsail if present
+    g2.motors.set_mainsail(100.0f);
+
     // send to motor
     g2.motors.set_throttle(throttle_out);
 

APMrover2/mode_hold.cpp

@@ -10,6 +10,9 @@ void ModeHold::update()
         rover.balancebot_pitch_control(throttle);
     }
 
+    // relax mainsail
+    g2.motors.set_mainsail(100.0f);
+
     // hold position - stop motors and center steering
     g2.motors.set_throttle(throttle);
     g2.motors.set_steering(0.0f);

APMrover2/mode_loiter.cpp

@@ -25,7 +25,9 @@ void ModeLoiter::update()
 
     // if within waypoint radius slew desired speed towards zero and use existing desired heading
     if (_distance_to_destination <= g2.loit_radius) {
-        _desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
+        // sailboats do not stop
+        const float desired_speed_within_radius = g2.motors.has_sail() ? 0.1f : 0.0f;
+        _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
         _yaw_error_cd = 0.0f;
     } else {
         // P controller with hard-coded gain to convert distance to desired speed

APMrover2/mode_manual.cpp

@@ -18,6 +18,9 @@ void ModeManual::update()
         rover.balancebot_pitch_control(desired_throttle);
     }
 
+    // set sailboat mainsail from throttle position
+    g2.motors.set_mainsail(desired_throttle);
+
     // copy RC scaled inputs to outputs
     g2.motors.set_throttle(desired_throttle);
     g2.motors.set_steering(desired_steering, false);

APMrover2/sailboat.cpp

@@ -0,0 +1,69 @@
+#include "Rover.h"
+
+/*
+To Do List
+ - Improve tacking in light winds and bearing away in strong wings
+ - consider drag vs lift sailing differences, ie upwind sail is like wing, dead down wind sail is like parachute
+ - max speed paramiter and contoller, for maping you may not want to go too fast
+ - mavlink sailing messages
+ - motor sailing, some boats may also have motor, we need to decide at what point we would be better of just motoring in low wind, or for a tight loiter, or to hit waypoint exactly, or if stuck head to wind, or to reverse...
+ - smart decision making, ie tack on windshifts, what to do if stuck head to wind
+ - some sailing codes track waves to try and 'surf' and to allow tacking on a flat bit, not sure if there is much gain to be had here
+ - add some sort of pitch monitoring to prevent nose diving in heavy weather
+ - pitch PID for hydrofoils
+ - more advanced sail control, ie twist
+ - independent sheeting for main and jib
+ - wing type sails with 'elevator' control
+ - tack on depth sounder info to stop sailing into shallow water on indirect sailing routes
+ - 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
+*/
+
+// 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)
+{
+    if (!g2.motors.has_sail()) {
+        return;
+    }
+
+    // relax sail if desired speed is zero
+    if (!is_positive(desired_speed)) {
+        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());
+    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;
+
+    // make sure between allowable range
+    mainsail_angle = constrain_float(mainsail_angle, g2.sail_angle_min, g2.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);
+
+    // 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;
+
+    mainsail = constrain_float((mainsail+pid_offset), 0.0f ,100.0f);
+    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
+{
+    // return 0 if not heading towards waypoint
+    if (!control_mode->is_autopilot_mode()) {
+        return 0.0f;
+    }
+
+    float speed;
+    if (!g2.attitude_control.get_forward_speed(speed)) {
+        return 0.0f;
+    }
+    return (speed * cosf(wrap_PI(radians(nav_controller->target_bearing_cd()) - ahrs.yaw)));
+}

APMrover2/sensors.cpp

@@ -241,6 +241,11 @@ void Rover::init_proximity(void)
     g2.proximity.set_rangefinder(&rangefinder);
 }
 
+void Rover::windvane_update()
+{
+    g2.windvane.update();
+}
+
 // 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.windvane.init();
+
     // init baro before we start the GCS, so that the CLI baro test works
     barometer.init();
 
@@ -335,6 +337,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
     // initialize simple mode heading
     rover.mode_simple.init_heading();
 
+    // save home heading for use in sail vehicles
+    rover.g2.windvane.record_home_headng();
+
     change_arm_state();
     return true;
 }