Skip to content

Commit

Permalink
Sailboat : Handle in Irons.
Browse files Browse the repository at this point in the history 
      Revert Ar_motorsUGV changes.
      Add SAIL_OPTIONS param; SAIL_PARAM options only available if Mainsheet RC input function is defined separate to throttle.
      Auto get out of in irons settable via SAIL_OPTIONS
  • Loading branch information
@kwikius
kwikius committed Feb 29, 2024
1 parent b20cab6 commit cec7595
Show file tree
Hide file tree
Showing 7 changed files with 173 additions and 134 deletions.
2 changes: 1 addition & 1 deletion Rover/crash_check.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ void Rover::crash_check()
if (!crashed && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity
(fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed
(fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN) ||
g2.motors.sailboat_in_irons())) {
g2.sailboat.in_irons())) {
crash_counter = 0;
return;
}
Expand Down
6 changes: 6 additions & 0 deletions Rover/mode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -415,6 +415,12 @@ float Mode::calc_speed_nudge(float target_speed, bool reversed)
void Mode::navigate_to_waypoint()
{
if ( g2.sailboat.in_irons()){
// rudder_percent ( 0.0f to 1.0f) sets the rudder angle used to turn the boat
// as it is blown backwards. Too much rudder angle can stall the rudders
// and slow the process of getting out of irons

//set rudder output to turn the boat in required direction
g2.motors.set_steering(g2.sailboat.get_in_irons_rudder() * 4500.0f,false);
return;
}
// apply speed nudge from pilot
Expand Down
2 changes: 1 addition & 1 deletion Rover/mode_auto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ void ModeAuto::_exit()
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
g2.motors.clear_sailboat_in_irons();
g2.sailboat.clear_in_irons();
}

void ModeAuto::update()
Expand Down
126 changes: 84 additions & 42 deletions Rover/sailboat.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,10 +103,33 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
// @User: Standard
AP_GROUPINFO("LOIT_RADIUS", 9, Sailboat, loit_radius, 5),

// @Param: OPTIONS
// @DisplayName: Sailboat options
// @Description: Sailboat setup options, Only available for separate mainsail RC channel.
/* @bitmask: 0:TODO Mainsail works as traditional RC sail in manual.
Range 1000 usec to 2000 usec.
1000 usec is fully sheeted in, 2000 usec is fully sheeted out */
// @bitmask: 1:reserved
// @bitmask: 2:reserved
// @bitmask: 3:reserved
/* @bitmask: 4:TODO mainsail channel modifies angle of attack in auto, acro.
MainSailRCIn range {1000 to 1900} usec:
Sail angle of attack = (1.0f - (MainsailChannelIn - 1000.0f)/900.0f) *
SAIL_ANGLE_IDEAL.
MainSailRCIn range {1900 +} usec: Sail relaxed. */
// @bitmask: 5:reserved
// @bitmask: 6:reserved
// @bitmask: 7:Detect and get out of "in Irons" in AUTO mode
// @User: Advanced
AP_GROUPINFO("OPTIONS", 10, Sailboat, options, 0),

AP_GROUPEND
};


namespace {
uint32_t constexpr getOutOfInIronsInAutoBit = 7U;
uint32_t constexpr getOutOfInIronsInAutoMask = (1U << getOutOfInIronsInAutoBit);
}
/*
constructor
*/
Expand Down Expand Up @@ -404,11 +427,11 @@ bool Sailboat::use_indirect_route(float desired_heading_cd) const
float Sailboat::calc_heading(float desired_heading_cd)
{
if ( in_irons()){
// in irons dont change the best heading,
// in irons dont change the best heading,
// keep slowly rotating in one direction
return degrees(in_irons_heading_rad) * 100.0f ;
}

if (!tack_enabled()) {
return desired_heading_cd;
}
Expand Down Expand Up @@ -479,7 +502,7 @@ float Sailboat::calc_heading(float desired_heading_cd)

// if tack triggered, calculate target heading
if (should_tack && (now - tack_clear_ms) > TACK_RETRY_TIME_MS) {

// calculate target heading for the new tack
const char* tack_text[] = {"port","stbd"};
int tack_idx = 0;
Expand Down Expand Up @@ -570,42 +593,39 @@ bool Sailboat::motor_assist_low_wind() const
(rover.g2.windvane.get_true_wind_speed() < sail_windspeed_min));
}

void Sailboat::set_in_irons()
{
// fix the desired heading while in irons
in_irons_heading_rad =
(currently_tacking)
? tack_heading_rad
: radians(rover.g2.wp_nav.oa_wp_bearing_cd() * 0.01)
;

// calculate which way to set the rudders to rotate the boat
// towards desired heading as it is being blown backwards
constexpr int rotate_clockwise = -1;
constexpr int rotate_anticlockwise = 1;
const auto rotate_sign =
(wrap_PI(in_irons_heading_rad - rover.ahrs.get_yaw()) >= 0.0f)
? rotate_clockwise
: rotate_anticlockwise
;

// we want the boat to rotate so relax the sails
// to prevent them acting as a weather vane
relax_sails();

// rudder_percent ( 0.0f to 1.0f) sets the rudder angle used to turn the boat
// as it is blown backwards. Too much rudder angle can stall the rudders
// and slow the process of getting out of irons
constexpr float rudder_percent = 2.0f/3.0f;

//set rudder output to turn the boat in required direction
rover.g2.motors.set_sailboat_in_irons( rotate_sign * 4500.0f * rudder_percent);
const char * tack_text = (rotate_sign == rotate_clockwise)? "port":"stbd";
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: In Irons rotate onto %s", tack_text);
}

bool Sailboat::in_irons()
{
auto set_in_irons = [this]()
{
// fix the desired heading while in irons
in_irons_heading_rad =
(currently_tacking)
? tack_heading_rad
: radians(rover.g2.wp_nav.oa_wp_bearing_cd() * 0.01)
;

// calculate which way to set the rudders to rotate the boat
// towards desired heading as it is being blown backwards
const int in_irons_rotate_sign =
(wrap_PI(in_irons_heading_rad - rover.ahrs.get_yaw()) >= 0.0f)
? Sailboat::rotate_clockwise
: Sailboat::rotate_anticlockwise
;

// we want the boat to rotate so relax the sails
// to prevent them acting as a weather vane
relax_sails();
// set the rudder for in irons steering output
in_irons_rudder = in_irons_rotate_sign * Sailboat::in_irons_rudder_percent;
is_in_irons = true;
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: In Irons rotate onto %s",
(in_irons_rotate_sign == Sailboat::rotate_clockwise)? "port":"stbd");
};

if (! get_out_of_in_irons_in_auto_enabled()){
clear_in_irons();
return false;
}
auto going_backwards = []()-> bool{
float speed;
// return false if we don't have a valid speed (e.g no gps fix)
Expand All @@ -626,9 +646,10 @@ bool Sailboat::in_irons()
auto headed = [=]()-> bool{
return abs(wind_dir_apparent) < sail_no_go;
};

// Already "in irons" or not?
if (!rover.g2.motors.sailboat_in_irons()){
//if (!rover.g2.motors.sailboat_in_irons()){
if (! is_in_irons){
// not currently in irons, check conditions to enter "in irons limbo"
if ( cannot_motorsail() && going_backwards() && headed() ){
// in_irons_limbo_start_ms == 0 means not yet "in irons limbo"
Expand All @@ -650,9 +671,30 @@ bool Sailboat::in_irons()
// be able to sheet in, fill sails and Go?
if ( abs(wind_dir_apparent) > sail_no_go){
in_irons_start_ms = 0;
rover.g2.motors.clear_sailboat_in_irons();
is_in_irons = false;
//rover.g2.motors.clear_sailboat_in_irons();
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Out of Irons");
}
}
return rover.g2.motors.sailboat_in_irons();
return is_in_irons ; //rover.g2.motors.sailboat_in_irons();
}

void Sailboat::clear_in_irons()
{
in_irons_start_ms = 0;
is_in_irons = false;
}

bool Sailboat::separate_mainsail_rcin_channel() const
{
return (channel_mainsail !=nullptr) &&
(channel_mainsail != rover.channel_throttle);
}

bool Sailboat::get_out_of_in_irons_in_auto_enabled() const
{
return separate_mainsail_rcin_channel()
&& (options & getOutOfInIronsInAutoMask) != 0U;
}


34 changes: 27 additions & 7 deletions Rover/sailboat.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,12 +92,20 @@ class Sailboat
// state machine. the state is updated at each call
// return true if sailboat is stuck "in irons"
bool in_irons();

// return the rudder to set to get out of irons
// between -1 and 1
float get_in_irons_rudder() const { return in_irons_rudder;}

// call this function only when changing modes to reset the in_irons
// Usually in_irons will clear itself by getting out of irons
// Can be called whether sailboat is in_irons or not
void clear_in_irons();
// is get out of in irons in auto mode enabled?
bool get_out_of_in_irons_in_auto_enabled() const;
// is mainsail on a separate rc in channel?
bool separate_mainsail_rcin_channel() const;
private:

// change sailboat state to in irons
void set_in_irons();

// true if motor is on to assist with slow tack
bool motor_assist_tack() const;

Expand All @@ -114,6 +122,7 @@ class Sailboat
AP_Float sail_windspeed_min;
AP_Float xtrack_max;
AP_Float loit_radius;
AP_Int32 options;

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
Expand All @@ -123,7 +132,18 @@ class Sailboat
uint32_t tack_clear_ms; // system time when tack was cleared
bool tack_assist; // true if we should use some throttle to assist tack
UseMotor motor_state; // current state of motor output
uint32_t in_irons_start_ms = 0U; // system time when in_irons started
float in_irons_heading_rad; // target heading in radians when in irons

// in irons variables ---
static constexpr int rotate_clockwise = -1;
static constexpr int rotate_anticlockwise = 1;
// the percentage of full rudder to apply in irons
// too much can stall the rudder and delay getting out of irons
static constexpr float in_irons_rudder_percent = 2.0f/3.0f;
// system time when in_irons started
uint32_t in_irons_start_ms = 0U;
// in irons state flag. Dont set directly
bool is_in_irons = false;
// value between -1 and 1 to send to steering when in irons
float in_irons_rudder = 0.0f;
// target heading in radians when in irons
float in_irons_heading_rad;
};
Loading

0 comments on commit cec7595

Please sign in to comment.