diff --git a/Rover/AP_MotorsUGV.cpp b/Rover/AP_MotorsUGV.cpp index ccaaddfd4f..9fe4602d6a 100644 --- a/Rover/AP_MotorsUGV.cpp +++ b/Rover/AP_MotorsUGV.cpp @@ -182,6 +182,8 @@ void AP_MotorsUGV::setup_servo_output() SRV_Channels::set_range(SRV_Channel::k_mainsail_sheet, 100); // wing sail -100 to 100 SRV_Channels::set_angle(SRV_Channel::k_wingsail_elevator, 100); + // mast rotation -100 to 100 + SRV_Channels::set_angle(SRV_Channel::k_mast_rotation, 100); } @@ -242,6 +244,12 @@ void AP_MotorsUGV::set_wingsail(float wingsail) _wingsail = constrain_float(wingsail, -100.0f, 100.0f); } +// set mast rotation input as a value from -100 to 100 +void AP_MotorsUGV::set_mast_rotation(float mast_rotation) +{ + _mast_rotation = constrain_float(mast_rotation, -100.0f, 100.0f); +} + // get slew limited throttle // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse // same as private slew_limit_throttle method (see below) but does not update throttle state @@ -270,7 +278,7 @@ bool AP_MotorsUGV::have_skid_steering() const // true if the vehicle has a mainsail bool AP_MotorsUGV::has_sail() const { - return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet) || SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator); + return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet) || SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator) || SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation); } void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) @@ -360,6 +368,9 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) { SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, pct); } + if (SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation)) { + SRV_Channels::set_output_scaled(SRV_Channel::k_mast_rotation, pct); + } break; } case MOTOR_TEST_LAST: @@ -423,6 +434,9 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) { SRV_Channels::set_output_pwm(SRV_Channel::k_wingsail_elevator, pwm); } + if (SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation)) { + SRV_Channels::set_output_pwm(SRV_Channel::k_mast_rotation, pwm); + } break; } default: @@ -885,6 +899,7 @@ void AP_MotorsUGV::output_sail() SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, _mainsail); SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, _wingsail); + SRV_Channels::set_output_scaled(SRV_Channel::k_mast_rotation, _mast_rotation); } // slew limit throttle for one iteration diff --git a/Rover/AP_MotorsUGV.h b/Rover/AP_MotorsUGV.h index 59382c7b03..2800aac4b0 100644 --- a/Rover/AP_MotorsUGV.h +++ b/Rover/AP_MotorsUGV.h @@ -84,6 +84,10 @@ public: void set_wingsail(float wingsail); float get_wingsail() const { return _wingsail; } + // set or get mast rotation input as a value from -100 to 100 + void set_mast_rotation(float mast_rotation); + float get_mast_rotation() const { return _mast_rotation; } + // get slew limited throttle // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse // same as private slew_limit_throttle method (see below) but does not update throttle state @@ -199,6 +203,7 @@ protected: float _walking_height; // requested height as a value from -1 to +1 float _mainsail; // requested mainsail input as a value from 0 to 100 float _wingsail; // requested wing sail input as a value in the range +- 100 + float _mast_rotation; // requested mast rotation input as a value in the range +- 100 // omni variables float _throttle_factor[AP_MOTORS_NUM_MOTORS_MAX]; diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 5d42e31280..4e25a4ca8a 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -307,9 +307,11 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled) // sailboats use special throttle and mainsail controller float mainsail_out = 0.0f; float wingsail_out = 0.0f; - rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out, wingsail_out); + float mast_rotation_out = 0.0f; + rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out, wingsail_out, mast_rotation_out); rover.g2.motors.set_mainsail(mainsail_out); rover.g2.motors.set_wingsail(wingsail_out); + rover.g2.motors.set_mast_rotation(mast_rotation_out); } else { // call speed or stop controller if (is_zero(target_speed) && !rover.is_balancebot()) { diff --git a/Rover/mode_manual.cpp b/Rover/mode_manual.cpp index 404a687e00..622e7ec162 100644 --- a/Rover/mode_manual.cpp +++ b/Rover/mode_manual.cpp @@ -29,9 +29,11 @@ void ModeManual::update() // set sailboat sails float desired_mainsail; float desired_wingsail; - g2.sailboat.get_pilot_desired_mainsail(desired_mainsail, desired_wingsail); + float desired_mast_rotation; + g2.sailboat.get_pilot_desired_mainsail(desired_mainsail, desired_wingsail, desired_mast_rotation); g2.motors.set_mainsail(desired_mainsail); g2.motors.set_wingsail(desired_wingsail); + g2.motors.set_mast_rotation(desired_wingsail); // copy RC scaled inputs to outputs g2.motors.set_throttle(desired_throttle); diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index 13ee059aa6..d55331e8ec 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -41,7 +41,7 @@ const AP_Param::GroupInfo Sailboat::var_info[] = { // @Param: ANGLE_MAX // @DisplayName: Sail max angle - // @Description: Mainsheet loose, angle between centerline and boom + // @Description: Mainsheet loose, angle between centerline and boom. For direct-control rotating masts, the rotation angle at SERVOx_MAX/_MIN; for rotating masts, this value can exceed 90 degrees if the linkages can physically rotate the mast past that angle. // @Units: deg // @Range: 0 90 // @Increment: 1 @@ -170,26 +170,29 @@ 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, float &wingsail_out) +void Sailboat::get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out, float &mast_rotation_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; + mast_rotation_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); + mast_rotation_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, float &wingsail_out) +void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttle_out, float &mainsail_out, float &wingsail_out, float &mast_rotation_out) { if (!sail_enabled()) { throttle_out = 0.0f; mainsail_out = 0.0f; wingsail_out = 0.0f; + mast_rotation_out = 0.0f; return; } @@ -212,12 +215,17 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl if (motor_state == UseMotor::USE_MOTOR_ALWAYS) { mainsail_out = 100.0f; wingsail_out = 0.0f; + mast_rotation_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); + // use PID controller to sheet out, this number is expected approximately in the 0 to 100 range (with default PIDs) + const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f; + + // get apparent wind, + is wind over starboard side, - is wind over port side + const float wind_dir_apparent = degrees(rover.g2.windvane.get_apparent_wind_direction_rad()); + const float wind_dir_apparent_abs = fabsf(wind_dir_apparent); + const float wind_dir_apparent_sign = is_negative(wind_dir_apparent) ? -1.0f : 1.0f; // // mainsail control @@ -227,12 +235,10 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl 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 - float wind_dir_apparent = fabsf(rover.g2.windvane.get_apparent_wind_direction_rad()); - wind_dir_apparent = degrees(wind_dir_apparent); + // Sails are sheeted the same on each side use abs wind direction // set the main sail to the ideal angle to the wind - float mainsail_angle = wind_dir_apparent -sail_angle_ideal; + float mainsail_angle = wind_dir_apparent_abs - sail_angle_ideal; // make sure between allowable range mainsail_angle = constrain_float(mainsail_angle,sail_angle_min, sail_angle_max); @@ -247,17 +253,58 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl // 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 auto trim, we only need to reduce power if we are tipping over, must also be trimmed for correct tack + // dont allow to reduce power to less than 0, ie not backwinding the sail to self-right + wingsail_out = (100.0f - MIN(pid_offset,100.0f)) * wind_dir_apparent_sign; - // wing sails must be trimmed for the correct tack - if (rover.g2.windvane.get_current_tack() == AP_WindVane::Sailboat_Tack::TACK_PORT) { + // wing sails can be used to go backwards, probably not recommended though + if (is_negative(desired_speed)) { wingsail_out *= -1.0f; } - // wing sails can be used to go backwards, probably not recommended though + // + // direct mast rotation control + // + if (!is_positive(desired_speed)) { - wingsail_out *= -1.0f; + // rotating sails can be used to reverse, but not in this version + mast_rotation_out = 0.0f; + } else { + + if (wind_dir_apparent_abs < sail_angle_ideal) { + // in irons, center the sail. + mast_rotation_out = 0.0f; + + } else { + + float mast_rotation_angle; + if (wind_dir_apparent_abs < (90.0f + sail_angle_ideal)) { + // use sail as a lift device, at ideal angle of attack, but depower to prevent excessive heel + // multiply pid_offset by 0.01 to keep the scaling in the same range as the other sail outputs + // this means the default PIDs should apply reasonably well to all sail types + mast_rotation_angle = wind_dir_apparent_abs - sail_angle_ideal * MAX(1.0f - pid_offset*0.01f,0.0f); + + // restore sign + mast_rotation_angle *= wind_dir_apparent_sign; + + } else { + // use sail as drag device, but avoid wagging the sail as the wind oscillates + // between 180 and -180 degrees + mast_rotation_angle = 90.0f; + if (wind_dir_apparent_abs > 135.0f) { + // wind is almost directly behind, keep wing on current tack + if (SRV_Channels::get_output_scaled(SRV_Channel::k_mast_rotation) < 0) { + mast_rotation_angle *= -1.0f; + } + } else { + // set the wing on the correct tack, so that is can be sheeted in if required + mast_rotation_angle *= wind_dir_apparent_sign; + } + } + + // linear interpolate servo displacement (-100 to 100) from mast rotation angle and restore sign + mast_rotation_out = linear_interpolate(-100.0f, 100.0f, mast_rotation_angle, -sail_angle_max, sail_angle_max); + } } } diff --git a/Rover/sailboat.h b/Rover/sailboat.h index de285e6c49..b7a5ae2e7d 100644 --- a/Rover/sailboat.h +++ b/Rover/sailboat.h @@ -38,10 +38,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 // wingsail_out is in the range -100 to 100, defaults to 0 - void get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out); + // mast_rotation_out is in the range -100 to 100, defaults to 0 + void get_pilot_desired_mainsail(float &mainsail_out, float &wingsail_out, float &mast_rotation_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, float &wingsail_out); + void get_throttle_and_mainsail_out(float desired_speed, float &throttle_out, float &mainsail_out, float &wingsail_out, float &mast_rotation_out); // Velocity Made Good, this is the speed we are traveling towards the desired destination float get_VMG() const;