From 3308a84acc61a6e45ce382bef4ba17ec0d4d4cb3 Mon Sep 17 00:00:00 2001 From: Peter Hall <33176108+IamPete1@users.noreply.github.com> Date: Sat, 25 May 2019 19:42:18 +0100 Subject: [PATCH] Rover: sailboat add motor-sailing --- APMrover2/Log.cpp | 4 +- APMrover2/Rover.h | 3 - APMrover2/mode.cpp | 30 ++++--- APMrover2/mode_loiter.cpp | 4 +- APMrover2/mode_manual.cpp | 6 +- APMrover2/radio.cpp | 3 + APMrover2/sailboat.cpp | 162 ++++++++++++++++++++++++++++++++------ APMrover2/sailboat.h | 29 ++++++- APMrover2/system.cpp | 4 +- 9 files changed, 192 insertions(+), 53 deletions(-) diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 82b13c1b15..c68626e01a 100644 --- a/APMrover2/Log.cpp +++ b/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; } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 8d8405f8af..cdda630c2f 100644 --- a/APMrover2/Rover.h +++ b/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; diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index fc3101d1c8..036bfe086e 100644 --- a/APMrover2/mode.cpp +++ b/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); } diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index 9f9286f39c..4bc89fcf40 100644 --- a/APMrover2/mode_loiter.cpp +++ b/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 diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index bd58798b82..24fa1748d2 100644 --- a/APMrover2/mode_manual.cpp +++ b/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); diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index f6e6a73af2..336626c24d 100644 --- a/APMrover2/radio.cpp +++ b/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(); diff --git a/APMrover2/sailboat.cpp b/APMrover2/sailboat.cpp index 3fe63cfd54..8965250ca2 100644 --- a/APMrover2/sailboat.cpp +++ b/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; +} diff --git a/APMrover2/sailboat.h b/APMrover2/sailboat.h index 32e6d32f17..1cb66b827e 100644 --- a/APMrover2/sailboat.h +++ b/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 }; diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index ca9b09fae3..bc0fb47303 100644 --- a/APMrover2/system.cpp +++ b/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();