From c9dae4a10edf59e386977a11bd9bddffcb5089f2 Mon Sep 17 00:00:00 2001 From: Andy Little Date: Tue, 6 Feb 2024 14:07:41 +0000 Subject: [PATCH 1/3] Rover Sailboat: refactor the mainsail/wingmast/mast_rotation logic and make non const sailboat functions private to the sailboat class. Saves around 128 bytes of text image size (in SITL anyway) --- Rover/mode.cpp | 11 +--- Rover/mode_hold.cpp | 3 +- Rover/mode_manual.cpp | 8 +-- Rover/sailboat.cpp | 133 ++++++++++++++++++++---------------------- Rover/sailboat.h | 79 ++++++++++++++----------- 5 files changed, 110 insertions(+), 124 deletions(-) diff --git a/Rover/mode.cpp b/Rover/mode.cpp index e54cc84f8ae28..c78cf0d77aff1 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -304,13 +304,7 @@ 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; - float wingsail_out = 0.0f; - 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); + rover.g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out); } else { // call speed or stop controller if (is_zero(target_speed) && !rover.is_balancebot()) { @@ -348,8 +342,7 @@ bool Mode::stop_vehicle() } // relax sails if present - g2.motors.set_mainsail(100.0f); - g2.motors.set_wingsail(0.0f); + g2.sailboat.relax_sails(); // send to motor g2.motors.set_throttle(throttle_out); diff --git a/Rover/mode_hold.cpp b/Rover/mode_hold.cpp index ea989157e34c1..a8d371ce16a3f 100644 --- a/Rover/mode_hold.cpp +++ b/Rover/mode_hold.cpp @@ -10,8 +10,7 @@ void ModeHold::update() } // relax mainsail - g2.motors.set_mainsail(100.0f); - g2.motors.set_wingsail(0.0f); + g2.sailboat.relax_sails(); // hold position - stop motors and center steering g2.motors.set_throttle(throttle); diff --git a/Rover/mode_manual.cpp b/Rover/mode_manual.cpp index 720295e15ae65..4b449bedb4987 100644 --- a/Rover/mode_manual.cpp +++ b/Rover/mode_manual.cpp @@ -29,13 +29,7 @@ void ModeManual::update() g2.motors.set_walking_height(desired_walking_height); // set sailboat sails - float desired_mainsail; - float 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); + g2.sailboat.set_pilot_desired_mainsail(); // copy RC scaled inputs to outputs g2.motors.set_throttle(desired_throttle); diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index 23f72e8a56af2..257e9a448b665 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -172,57 +172,24 @@ 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, float &mast_rotation_out) +// @brief decode pilot mainsail input in manual modes and update the various sail values for different sail types +// ready for SRV_Channel output. +void Sailboat::set_pilot_desired_mainsail() { // 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; + relax_sails(); + }else{ + rover.g2.motors.set_mainsail(constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f)); + rover.g2.motors.set_wingsail(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f)); + rover.g2.motors.set_mast_rotation(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f)); } - 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, float &mast_rotation_out) +/// @brief Set mainsail in auto modes +/// @param[in] desired_speed desired speed (in m/s) only used to detect desired direction +void Sailboat::set_auto_mainsail(float desired_speed) { - if (!sail_enabled()) { - throttle_out = 0.0f; - mainsail_out = 0.0f; - wingsail_out = 0.0f; - mast_rotation_out = 0.0f; - return; - } - - // run speed controller if motor is forced on or motor assistance is required for low speeds or tacking - if ((motor_state == UseMotor::USE_MOTOR_ALWAYS) || - motor_assist_tack() || - motor_assist_low_wind()) { - // 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; - } - - // if we are motoring relax sails - 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, 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; @@ -232,55 +199,41 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl const float wind_dir_apparent_sign = is_negative(wind_dir_apparent) ? -1.0f : 1.0f; // - // mainsail control + // mainsail control. // - + // mainsail_out represents a range from 0 to 100 + float mainsail_out = 100.f; // main sails cannot be used to reverse - if (!is_positive(desired_speed)) { - mainsail_out = 100.0f; - } else { + if (is_positive(desired_speed)) { // 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_abs - sail_angle_ideal; - - // make sure between allowable range - mainsail_angle = constrain_float(mainsail_angle,sail_angle_min, sail_angle_max); + const float mainsail_angle = + constrain_float(wind_dir_apparent_abs - sail_angle_ideal,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); + const float mainsail_base = linear_interpolate(0.0f, 100.0f, mainsail_angle,sail_angle_min,sail_angle_max); mainsail_out = constrain_float((mainsail_base + pid_offset), 0.0f ,100.0f); } - + rover.g2.motors.set_mainsail(mainsail_out); // // wingsail control - // - // 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 can be used to go backwards, probably not recommended though - if (is_negative(desired_speed)) { - wingsail_out *= -1.0f; - } - + const float wing_sail_out_sign = is_negative(desired_speed)?-1.f:1.f; + const float wingsail_out = (100.0f - MIN(pid_offset,100.0f)) * wind_dir_apparent_sign * wing_sail_out_sign; + rover.g2.motors.set_wingsail(wingsail_out); // // direct mast rotation control // - - if (!is_positive(desired_speed)) { + float mast_rotation_out = 0.f; + if (is_positive(desired_speed)) { // 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 @@ -310,7 +263,45 @@ void Sailboat::get_throttle_and_mainsail_out(float desired_speed, float &throttl mast_rotation_out = linear_interpolate(-100.0f, 100.0f, mast_rotation_angle, -sail_angle_max, sail_angle_max); } } + rover.g2.motors.set_mast_rotation(mast_rotation_out); +} + +void Sailboat::relax_sails() +{ + rover.g2.motors.set_mainsail(100.0f); + rover.g2.motors.set_wingsail(0.f); + rover.g2.motors.set_mast_rotation(0.f); +} + +// calculate throttle and mainsail angle required to attain desired speed (in m/s) +void Sailboat::get_throttle_and_set_mainsail(float desired_speed, float &throttle_out) +{ + if (!sail_enabled()) { + relax_sails(); + throttle_out = 0.0f; + return; + } + // run speed controller if motor is forced on or motor assistance is required for low speeds or tacking + if ((motor_state == UseMotor::USE_MOTOR_ALWAYS) || + motor_assist_tack() || + motor_assist_low_wind()) { + // 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; + } + + if (motor_state == UseMotor::USE_MOTOR_ALWAYS) { + relax_sails(); + }else{ + set_auto_mainsail(desired_speed); + } } // Velocity Made Good, this is the speed we are traveling towards the desired destination diff --git a/Rover/sailboat.h b/Rover/sailboat.h index b7a5ae2e7d3bc..dcc30086d46f7 100644 --- a/Rover/sailboat.h +++ b/Rover/sailboat.h @@ -18,6 +18,15 @@ */ class Sailboat { + + friend class Rover; + friend class Mode; + friend class ModeManual; + friend class ModeHold; + friend class ModeLoiter; + friend class ModeAcro; + friend class RC_Channel_Rover; + public: // constructor @@ -29,45 +38,15 @@ class Sailboat // true if sailboat navigation (aka tacking) is enabled bool tack_enabled() const; - // setup - void init(); - - // 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 - // wingsail_out is in the range -100 to 100, defaults to 0 - // 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, float &mast_rotation_out); - // Velocity Made Good, this is the speed we are traveling towards the desired destination float get_VMG() const; - // handle user initiated tack while in acro mode - void handle_tack_request_acro(); - - // return target heading in radians when tacking (only used in acro) - float get_tack_heading_rad(); - - // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc) - void handle_tack_request_auto(); - - // clear tacking state variables - void clear_tack(); - // returns true if boat is currently tacking bool tacking() const; // returns true if sailboat should take a indirect navigation route to go upwind bool use_indirect_route(float desired_heading_cd) const; - // calculate the heading to sail on if we cant go upwind - float calc_heading(float desired_heading_cd); - // states of USE_MOTOR parameter and motor_state variable enum class UseMotor { USE_MOTOR_NEVER = 0, @@ -75,17 +54,47 @@ class Sailboat USE_MOTOR_ALWAYS = 2 }; + // return sailboat loiter radius + float get_loiter_radius() const {return loit_radius;} + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // handle user initiated tack while in acro mode + void handle_tack_request_acro(); + + // setup + void init(); + + // initialise rc input (channel_mainsail) + void init_rc_in(); + + + // return target heading in radians when tacking (only used in acro) + float get_tack_heading_rad(); + + // clear tacking state variables + void clear_tack(); + + // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc) + void handle_tack_request_auto(); + // set state of motor // if report_failure is true a message will be sent to all GCSs void set_motor_state(UseMotor state, bool report_failure = true); - // var_info for holding Parameter information - static const struct AP_Param::GroupInfo var_info[]; + // calculate the heading to sail on if we cant go upwind + float calc_heading(float desired_heading_cd); - // return sailboat loiter radius - float get_loiter_radius() const {return loit_radius;} + void set_pilot_desired_mainsail(); -private: + void set_auto_mainsail(float desired_speed); + void relax_sails(); + + // calculate throttle and set sail + void get_throttle_and_set_mainsail(float desired_speed, float &throttle_out); // true if motor is on to assist with slow tack bool motor_assist_tack() const; From 66fa60274e7133409b88d36f05683229025fb7ba Mon Sep 17 00:00:00 2001 From: Andy Little Date: Wed, 7 Feb 2024 20:46:08 +0000 Subject: [PATCH 2/3] Rover Sailboat: reset and Tidy up https://github.com/ArduPilot/ardupilot/pull/26152 --- Rover/sailboat.cpp | 27 +++++++++++++-------------- Rover/sailboat.h | 1 - 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index 257e9a448b665..7fb957e06c628 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -172,14 +172,14 @@ void Sailboat::init_rc_in() } } -// @brief decode pilot mainsail input in manual modes and update the various sail values for different sail types -// ready for SRV_Channel output. +/// @brief decode pilot mainsail input in manual modes and update the various +/// sail actuator values for different sail types ready for SRV_Channel output. void Sailboat::set_pilot_desired_mainsail() { // no RC input means mainsail is moved to trim if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) { relax_sails(); - }else{ + } else { rover.g2.motors.set_mainsail(constrain_float(channel_mainsail->get_control_in(), 0.0f, 100.0f)); rover.g2.motors.set_wingsail(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f)); rover.g2.motors.set_mast_rotation(constrain_float(channel_mainsail->get_control_in(), -100.0f, 100.0f)); @@ -202,7 +202,7 @@ void Sailboat::set_auto_mainsail(float desired_speed) // mainsail control. // // mainsail_out represents a range from 0 to 100 - float mainsail_out = 100.f; + float mainsail_out = 100.0f; // main sails cannot be used to reverse if (is_positive(desired_speed)) { // Sails are sheeted the same on each side use abs wind direction @@ -221,13 +221,13 @@ void Sailboat::set_auto_mainsail(float desired_speed) // 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 // wing sails can be used to go backwards, probably not recommended though - const float wing_sail_out_sign = is_negative(desired_speed)?-1.f:1.f; + const float wing_sail_out_sign = is_negative(desired_speed) ? -1.0f : 1.0f; const float wingsail_out = (100.0f - MIN(pid_offset,100.0f)) * wind_dir_apparent_sign * wing_sail_out_sign; rover.g2.motors.set_wingsail(wingsail_out); // // direct mast rotation control // - float mast_rotation_out = 0.f; + float mast_rotation_out = 0.0f; if (is_positive(desired_speed)) { // rotating sails can be used to reverse, but not in this version if (wind_dir_apparent_abs < sail_angle_ideal) { @@ -258,7 +258,6 @@ void Sailboat::set_auto_mainsail(float desired_speed) 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); } @@ -268,9 +267,9 @@ void Sailboat::set_auto_mainsail(float desired_speed) void Sailboat::relax_sails() { - rover.g2.motors.set_mainsail(100.0f); - rover.g2.motors.set_wingsail(0.f); - rover.g2.motors.set_mast_rotation(0.f); + rover.g2.motors.set_mainsail(100.0f); + rover.g2.motors.set_wingsail(0.0f); + rover.g2.motors.set_mast_rotation(0.0f); } // calculate throttle and mainsail angle required to attain desired speed (in m/s) @@ -298,9 +297,9 @@ void Sailboat::get_throttle_and_set_mainsail(float desired_speed, float &throttl } if (motor_state == UseMotor::USE_MOTOR_ALWAYS) { - relax_sails(); - }else{ - set_auto_mainsail(desired_speed); + relax_sails(); + } else { + set_auto_mainsail(desired_speed); } } @@ -441,7 +440,7 @@ float Sailboat::calc_heading(float desired_heading_cd) // check for user requested tack uint32_t now = AP_HAL::millis(); - if (tack_request_ms != 0 && !should_tack && !currently_tacking) { + if (tack_request_ms != 0 && !should_tack && !currently_tacking) { // set should_tack flag is user requested tack within last 0.5 sec should_tack = ((now - tack_request_ms) < 500); tack_request_ms = 0; diff --git a/Rover/sailboat.h b/Rover/sailboat.h index dcc30086d46f7..b6d529c44a577 100644 --- a/Rover/sailboat.h +++ b/Rover/sailboat.h @@ -71,7 +71,6 @@ class Sailboat // initialise rc input (channel_mainsail) void init_rc_in(); - // return target heading in radians when tacking (only used in acro) float get_tack_heading_rad(); From 2e9f0c516c5f1e093454b704f985b4446128f252 Mon Sep 17 00:00:00 2001 From: Andy Little Date: Tue, 20 Feb 2024 11:36:32 +0000 Subject: [PATCH 3/3] Rover: Sailboat: revert friends and private Sailboat functions. See https://github.com/ArduPilot/ardupilot/pull/26152#discussion_r1495156661 --- Rover/sailboat.h | 74 +++++++++++++++++++++++------------------------- 1 file changed, 35 insertions(+), 39 deletions(-) diff --git a/Rover/sailboat.h b/Rover/sailboat.h index b6d529c44a577..e0f9d06f30403 100644 --- a/Rover/sailboat.h +++ b/Rover/sailboat.h @@ -18,15 +18,6 @@ */ class Sailboat { - - friend class Rover; - friend class Mode; - friend class ModeManual; - friend class ModeHold; - friend class ModeLoiter; - friend class ModeAcro; - friend class RC_Channel_Rover; - public: // constructor @@ -38,15 +29,39 @@ class Sailboat // true if sailboat navigation (aka tacking) is enabled bool tack_enabled() const; + // setup + void init(); + + // initialise rc input (channel_mainsail) + void init_rc_in(); + + // calculate throttle and set sail + void get_throttle_and_set_mainsail(float desired_speed, float &throttle_out); + // Velocity Made Good, this is the speed we are traveling towards the desired destination float get_VMG() const; + // handle user initiated tack while in acro mode + void handle_tack_request_acro(); + + // return target heading in radians when tacking (only used in acro) + float get_tack_heading_rad(); + + // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc) + void handle_tack_request_auto(); + + // clear tacking state variables + void clear_tack(); + // returns true if boat is currently tacking bool tacking() const; // returns true if sailboat should take a indirect navigation route to go upwind bool use_indirect_route(float desired_heading_cd) const; + // calculate the heading to sail on if we cant go upwind + float calc_heading(float desired_heading_cd); + // states of USE_MOTOR parameter and motor_state variable enum class UseMotor { USE_MOTOR_NEVER = 0, @@ -54,46 +69,27 @@ class Sailboat USE_MOTOR_ALWAYS = 2 }; - // return sailboat loiter radius - float get_loiter_radius() const {return loit_radius;} - - // var_info for holding Parameter information - static const struct AP_Param::GroupInfo var_info[]; - -private: - - // handle user initiated tack while in acro mode - void handle_tack_request_acro(); - - // setup - void init(); - - // initialise rc input (channel_mainsail) - void init_rc_in(); - - // return target heading in radians when tacking (only used in acro) - float get_tack_heading_rad(); - - // clear tacking state variables - void clear_tack(); - - // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc) - void handle_tack_request_auto(); - // set state of motor // if report_failure is true a message will be sent to all GCSs void set_motor_state(UseMotor state, bool report_failure = true); - // calculate the heading to sail on if we cant go upwind - float calc_heading(float desired_heading_cd); + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + // return sailboat loiter radius + float get_loiter_radius() const {return loit_radius;} + + // set mainsail according to pilot input void set_pilot_desired_mainsail(); + // set mainsail in auto modes void set_auto_mainsail(float desired_speed); + + // as far as possible stop sails driving the boat + // by feathering or sheeting right out void relax_sails(); - // calculate throttle and set sail - void get_throttle_and_set_mainsail(float desired_speed, float &throttle_out); +private: // true if motor is on to assist with slow tack bool motor_assist_tack() const;