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

Rover: direct-rotation sail mast control

This commit is contained in:
Ari Krupnik 2021-04-27 14:50:36 -05:00 committed by Peter Hall
parent 02969451da
commit f5491433f6
6 changed files with 93 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Rover/AP_MotorsUGV.cpp
View File

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

5
Rover/AP_MotorsUGV.h
View File

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

4
Rover/mode.cpp
View File

@ -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()) {

4
Rover/mode_manual.cpp
View File

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

79
Rover/sailboat.cpp
View File

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

5
Rover/sailboat.h
View File

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