From fbd502d7ab1a2444f178cd9b430c3ca9b8d8e1fc Mon Sep 17 00:00:00 2001 From: Peter Hall <33176108+IamPete1@users.noreply.github.com> Date: Fri, 27 Sep 2019 20:33:33 +0100 Subject: [PATCH] Rover: add wing sail support --- APMrover2/Log.cpp | 5 ++-- APMrover2/mode.cpp | 7 ++++-- APMrover2/mode_hold.cpp | 1 + APMrover2/mode_manual.cpp | 6 +++-- APMrover2/sailboat.cpp | 48 +++++++++++++++++++++++++++++++-------- APMrover2/sailboat.h | 5 ++-- 6 files changed, 54 insertions(+), 18 deletions(-) diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index ef1b8f61c5..176cc93f76 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -138,14 +138,15 @@ void Rover::Log_Write_Sail() wind_speed_true = g2.windvane.get_true_wind_speed(); wind_speed_apparent = g2.windvane.get_apparent_wind_speed(); } - logger.Write("SAIL", "TimeUS,WindDirTrue,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG", - "shhnn%n", "F000000", "Qffffff", + logger.Write("SAIL", "TimeUS,WndDrTru,WndDrApp,WndSpdTru,WndSpdApp,MainOut,WingOut,VMG", + "shhnn%%n", "F0000000", "Qfffffff", 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)g2.motors.get_wingsail(), (double)g2.sailboat.get_VMG()); } diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 371fd313c6..b7e13a4de1 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -250,8 +250,10 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled) 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); + float wingsail_out = 0.0f; + rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out, wingsail_out); rover.g2.motors.set_mainsail(mainsail_out); + rover.g2.motors.set_wingsail(wingsail_out); } else { // call speed or stop controller if (is_zero(target_speed) && !rover.is_balancebot()) { @@ -286,8 +288,9 @@ bool Mode::stop_vehicle() 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); } - // relax mainsail if present + // relax sails if present g2.motors.set_mainsail(100.0f); + g2.motors.set_wingsail(0.0f); // send to motor g2.motors.set_throttle(throttle_out); diff --git a/APMrover2/mode_hold.cpp b/APMrover2/mode_hold.cpp index 95ff35febd..df2759a51c 100644 --- a/APMrover2/mode_hold.cpp +++ b/APMrover2/mode_hold.cpp @@ -12,6 +12,7 @@ void ModeHold::update() // relax mainsail g2.motors.set_mainsail(100.0f); + g2.motors.set_wingsail(0.0f); // hold position - stop motors and center steering g2.motors.set_throttle(throttle); diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index 24fa1748d2..81959317bf 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -18,10 +18,12 @@ void ModeManual::update() rover.balancebot_pitch_control(desired_throttle); } - // set sailboat mainsail + // set sailboat sails float desired_mainsail; - g2.sailboat.get_pilot_desired_mainsail(desired_mainsail); + float desired_wingsail; + g2.sailboat.get_pilot_desired_mainsail(desired_mainsail, desired_wingsail); g2.motors.set_mainsail(desired_mainsail); + g2.motors.set_wingsail(desired_wingsail); // copy RC scaled inputs to outputs g2.motors.set_throttle(desired_throttle); diff --git a/APMrover2/sailboat.cpp b/APMrover2/sailboat.cpp index 94cecfbed8..c079648b3e 100644 --- a/APMrover2/sailboat.cpp +++ b/APMrover2/sailboat.cpp @@ -10,14 +10,12 @@ To Do List - consider drag vs lift sailing differences, ie upwind sail is like wing, dead down wind sail is like parachute - max speed paramiter and controller, for mapping 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 */ @@ -172,23 +170,26 @@ void Sailboat::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 Sailboat::get_pilot_desired_mainsail(float &mainsail_out) +void Sailboat::get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out) { // no RC input means mainsail is moved to trim if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) { mainsail_out = 100.0f; + wingsail_out = 0.0f; return; } mainsail_out = constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f); + wingsail_out = constrain_float(channel_mainsail->get_control_in(), -100.0f, 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) +void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttle_out, float &mainsail_out, float &wingsail_out) { if (!sail_enabled()) { throttle_out = 0.0f; mainsail_out = 0.0f; + wingsail_out = 0.0f; return; } @@ -207,11 +208,23 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl throttle_out = 0.0f; } + // if we are motoring relax sails + if (motor_state == UseMotor::USE_MOTOR_ALWAYS) { + mainsail_out = 100.0f; + wingsail_out = 0.0f; + return; + } + + // use PID controller to sheet out + float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f; + pid_offset = constrain_float(pid_offset, 0.0f, 100.0f); + // // mainsail control // - // if we are motoring or attempting to reverse relax the sail - if (motor_state == UseMotor::USE_MOTOR_ALWAYS || !is_positive(desired_speed)) { + + // main sails cannot be used to reverse + if (!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 @@ -227,11 +240,26 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl // 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; - mainsail_out = constrain_float((mainsail_base + pid_offset), 0.0f ,100.0f); } + + // + // wingsail control + // + + // wing sails auto trim, we only need to reduce power if we are tipping over + wingsail_out = 100.0f - pid_offset; + + // wing sails must be trimmed for the correct tack + if (rover.g2.windvane.get_current_tack() == AP_WindVane::Sailboat_Tack::TACK_PORT) { + wingsail_out *= -1.0f; + } + + // wing sails can be used to go backwards, probably not recommended though + if (!is_positive(desired_speed)) { + wingsail_out *= -1.0f; + } + } // Velocity Made Good, this is the speed we are traveling towards the desired destination @@ -333,7 +361,7 @@ float Sailboat::calc_heading(float desired_heading_cd) } bool should_tack = false; - // find witch tack we are on + // find which tack we are on const AP_WindVane::Sailboat_Tack current_tack = rover.g2.windvane.get_current_tack(); // convert desired heading to radians diff --git a/APMrover2/sailboat.h b/APMrover2/sailboat.h index 5fea04c6e1..de285e6c49 100644 --- a/APMrover2/sailboat.h +++ b/APMrover2/sailboat.h @@ -37,10 +37,11 @@ public: // 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); + // wingsail_out is in the range -100 to 100, defaults to 0 + void get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_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); + void get_throttle_and_mainsail_out(float desired_speed, float &throttle_out, float &mainsail_out, float &wingsail_out); // Velocity Made Good, this is the speed we are traveling towards the desired destination float get_VMG() const;