Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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)
This commit is contained in:
Andy Little 2024-02-06 14:07:41 +00:00 committed by Randy Mackay
parent 3ab5f8139c
commit 5c26b40c7e
5 changed files with 111 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Rover/mode.cpp
View File

@ -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);

3
Rover/mode_hold.cpp
View File

@ -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);

8
Rover/mode_manual.cpp
View File

@ -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);

135
Rover/sailboat.cpp
View File

@ -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;
}
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)
{
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);
relax_sails();
}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;
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));
}
}
/// @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)
{
// 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

81
Rover/sailboat.h
View File

@ -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 @@ public:
// 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,18 +54,48 @@ public:
USE_MOTOR_ALWAYS = 2
};
// 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);
// 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[];
// return sailboat loiter radius
float get_loiter_radius() const {return loit_radius;}
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);
void set_pilot_desired_mainsail();
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;