aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: sailboat add motor-sailing

Browse Source
This commit is contained in:
Peter Hall 2019-05-25 19:42:18 +01:00 committed by Randy Mackay
parent 3dd0ebaf0d
commit 3308a84acc
9 changed files with 193 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/Log.cpp
View File

@ -28,7 +28,7 @@ void Rover::Log_Write_Attitude()
}
// log heel to sail control for sailboats
if (rover.g2.sailboat.enabled()) {
if (rover.g2.sailboat.sail_enabled()) {
logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -123,7 +123,7 @@ void Rover::Log_Write_Nav_Tuning()
void Rover::Log_Write_Sail()
{
// only log sail if present
if (!rover.g2.sailboat.enabled()) {
if (!rover.g2.sailboat.sail_enabled()) {
return;
}

3
APMrover2/Rover.h
View File

@ -486,9 +486,6 @@ public:
// Simple mode
float simple_sin_yaw;
// sailboat enabled
bool get_sailboat_enable() { return g2.sailboat.enabled(); }
};
extern const AP_HAL::HAL& hal;

30
APMrover2/mode.cpp
View File

@ -272,24 +272,28 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
}
// call throttle controller and convert output to -100 to +100 range
float throttle_out;
float throttle_out = 0.0f;
// call speed or stop controller
if (is_zero(target_speed) && !rover.is_balancebot()) {
bool stopped;
throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
if (rover.g2.sailboat.sail_enabled()) {
// sailboats use special throttle and mainsail controller
float mainsail_out = 0.0f;
rover.g2.sailboat.get_throttle_and_mainsail_out(target_speed, throttle_out, mainsail_out);
rover.g2.motors.set_mainsail(mainsail_out);
} else {
throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
}
// call speed or stop controller
if (is_zero(target_speed) && !rover.is_balancebot()) {
bool stopped;
throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
} else {
throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
}
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(throttle_out);
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(throttle_out);
}
}
// update mainsail position if present
rover.g2.sailboat.update_mainsail(target_speed);
// send to motor
g2.motors.set_throttle(throttle_out);
}

4
APMrover2/mode_loiter.cpp
View File

@ -24,8 +24,8 @@ void ModeLoiter::update()
// if within loiter radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g2.loit_radius) {
// sailboats do not stop
const float desired_speed_within_radius = rover.g2.sailboat.enabled() ? 0.1f : 0.0f;
// sailboats should not stop unless motoring
const float desired_speed_within_radius = rover.g2.sailboat.nav_enabled() ? 0.1f : 0.0f;
_desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
} else {
// P controller with hard-coded gain to convert distance to desired speed

6
APMrover2/mode_manual.cpp
View File

@ -18,8 +18,10 @@ void ModeManual::update()
rover.balancebot_pitch_control(desired_throttle);
}
// set sailboat mainsail from throttle position
g2.motors.set_mainsail(desired_throttle);
// set sailboat mainsail
float desired_mainsail;
g2.sailboat.get_pilot_desired_mainsail(desired_mainsail);
g2.motors.set_mainsail(desired_mainsail);
// copy RC scaled inputs to outputs
g2.motors.set_throttle(desired_throttle);

3
APMrover2/radio.cpp
View File

@ -17,6 +17,9 @@ void Rover::set_control_channels(void)
channel_lateral->set_angle(100);
}
// sailboat rc input init
g2.sailboat.init_rc_in();
// Allow to reconfigure output when not armed
if (!arming.is_armed()) {
g2.motors.setup_servo_output();

164
APMrover2/sailboat.cpp
View File

@ -25,7 +25,7 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable Sailboat
// @Description: This enables Sailboat functionality
// @Values: 0:Disable,1:Enable
// @Values: 0:Disable,1:Enable sail assist only,2:Enable
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("ENABLE", 1, Sailboat, enable, 0, AP_PARAM_FLAG_ENABLE),
@ -75,6 +75,15 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
// @User: Standard
AP_GROUPINFO("NO_GO_ANGLE", 6, Sailboat, sail_no_go, 45),
// @Param: WNDSPD_MIN
// @DisplayName: Sailboat minimum wind speed to sail in
// @Description: Sailboat minimum wind speed to continue sail in, at lower wind speeds the sailboat will motor if one is fitted
// @Units: m/s
// @Range: 0 5
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_assist_windspeed, 0),
AP_GROUPEND
};
@ -87,48 +96,111 @@ Sailboat::Sailboat()
AP_Param::setup_object_defaults(this, var_info);
}
// Should we use sailboat navigation?
bool Sailboat::nav_enabled() const
{
return (enable == 2) &&
(throttle_state != Sailboat_Throttle::FORCE_MOTOR) &&
(!throttle_assist() || tack_assist);
}
void Sailboat::init()
{
// sailboat defaults
if (enabled()) {
if (sail_enabled()) {
rover.g2.crash_angle.set_default(0);
}
if (nav_enabled()) {
rover.g2.loit_type.set_default(1);
rover.g2.loit_radius.set_default(5);
rover.g2.wp_nav.set_default_overshoot(10);
}
// if we have a throttle of some sort allow to use it
if (rover.g2.motors.have_skid_steering() ||
SRV_Channels::function_assigned(SRV_Channel::k_throttle) ||
rover.get_frame_type() != rover.g2.motors.frame_type::FRAME_TYPE_UNDEFINED) {
throttle_state = Sailboat_Throttle::ASSIST;
}
}
// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
void Sailboat::update_mainsail(float desired_speed)
// initialise rc input (channel_mainsail), may be called intermittently
void Sailboat::init_rc_in()
{
if (!enabled()) {
// get auxiliary throttle value
RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MAINSAIL);
if (rc_ptr != nullptr) {
// use aux as sail input if defined
channel_mainsail = rc_ptr;
channel_mainsail->set_angle(100);
channel_mainsail->set_default_dead_zone(30);
} else {
// use throttle channel
channel_mainsail = rover.channel_throttle;
}
}
// 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)
{
// no RC input means mainsail is moved to trim
if ((rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (channel_mainsail == nullptr)) {
mainsail_out = 100.0f;
return;
}
mainsail_out = constrain_float(channel_mainsail->get_control_in(), 0.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)
{
if (!sail_enabled()) {
throttle_out = 0.0f;
mainsail_out = 0.0f;
return;
}
// relax sail if desired speed is zero
if (!is_positive(desired_speed)) {
rover.g2.motors.set_mainsail(100.0f);
return;
// run speed controller if motor is forced on or if assistance is enabled for low speeds or tacking
if ((throttle_state == Sailboat_Throttle::FORCE_MOTOR) || throttle_assist()) {
// 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;
}
// + 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);
//
// mainsail control
//
// if we are motoring or attempting to reverse relax the sail
if (throttle_state == Sailboat_Throttle::FORCE_MOTOR || !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);
// set the main sail to the ideal angle to the wind
float mainsail_angle = wind_dir_apparent -sail_angle_ideal;
// set the main sail to the ideal angle to the wind
float mainsail_angle = wind_dir_apparent -sail_angle_ideal;
// make sure between allowable range
mainsail_angle = constrain_float(mainsail_angle,sail_angle_min, sail_angle_max);
// make sure between allowable range
mainsail_angle = constrain_float(mainsail_angle,sail_angle_min, sail_angle_max);
// linear interpolate mainsail value (0 to 100) from wind angle mainsail_angle
float mainsail = linear_interpolate(0.0f, 100.0f, mainsail_angle,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);
// use PID controller to sheet out
const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
// use PID controller to sheet out
const float pid_offset = rover.g2.attitude_control.get_sail_out_from_heel(radians(sail_heel_angle_max), rover.G_Dt) * 100.0f;
mainsail = constrain_float((mainsail+pid_offset), 0.0f ,100.0f);
rover.g2.motors.set_mainsail(mainsail);
mainsail_out = constrain_float((mainsail_base + pid_offset), 0.0f ,100.0f);
}
}
// Velocity Made Good, this is the speed we are traveling towards the desired destination
@ -152,9 +224,14 @@ float Sailboat::get_VMG() const
// handle user initiated tack while in acro mode
void Sailboat::handle_tack_request_acro()
{
if (!nav_enabled() || currently_tacking) {
return;
}
// set tacking heading target to the current angle relative to the true wind but on the new tack
currently_tacking = true;
tack_heading_rad = wrap_2PI(rover.ahrs.yaw + 2.0f * wrap_PI((rover.g2.windvane.get_absolute_wind_direction_rad() - rover.ahrs.yaw)));
auto_tack_request_ms = AP_HAL::millis();
}
// return target heading in radians when tacking (only used in acro)
@ -166,6 +243,10 @@ float Sailboat::get_tack_heading_rad() const
// handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
void Sailboat::handle_tack_request_auto()
{
if (!nav_enabled() || currently_tacking) {
return;
}
// record time of request for tack. This will be processed asynchronously by sailboat_calc_heading
auto_tack_request_ms = AP_HAL::millis();
}
@ -174,20 +255,21 @@ void Sailboat::handle_tack_request_auto()
void Sailboat::clear_tack()
{
currently_tacking = false;
tack_assist = false;
auto_tack_request_ms = 0;
}
// returns true if boat is currently tacking
bool Sailboat::tacking() const
{
return enabled() && currently_tacking;
return nav_enabled() && currently_tacking;
}
// returns true if sailboat should take a indirect navigation route to go upwind
// desired_heading should be in centi-degrees
bool Sailboat::use_indirect_route(float desired_heading_cd) const
{
if (!enabled()) {
if (!nav_enabled()) {
return false;
}
@ -202,7 +284,7 @@ bool Sailboat::use_indirect_route(float desired_heading_cd) const
// this function assumes the caller has already checked sailboat_use_indirect_route(desired_heading_cd) returned true
float Sailboat::calc_heading(float desired_heading_cd)
{
if (!enabled()) {
if (!nav_enabled()) {
return desired_heading_cd;
}
@ -266,8 +348,13 @@ float Sailboat::calc_heading(float desired_heading_cd)
clear_tack();
} else if ((now - auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
// tack has taken too long
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking timed out");
clear_tack();
if ((throttle_state == Sailboat_Throttle::ASSIST) && (now - auto_tack_start_ms) < (3.0f * SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) {
// if we have throttle available use it for another two time periods to get the tack done
tack_assist = true;
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking timed out");
clear_tack();
}
}
// return tack target heading
return degrees(tack_heading_rad) * 100.0f;
@ -280,3 +367,26 @@ float Sailboat::calc_heading(float desired_heading_cd)
return degrees(right_no_go_heading_rad) * 100.0f;
}
}
// should we use the throttle?
bool Sailboat::throttle_assist() const
{
// throttle is assist is disabled
if (throttle_state != Sailboat_Throttle::ASSIST) {
return false;
}
// assist with a tack
if (tack_assist) {
return true;
}
// wind speed is less than sailing cut-off
if (!is_zero(sail_assist_windspeed) &&
rover.g2.windvane.wind_speed_enabled() &&
rover.g2.windvane.get_true_wind_speed() < sail_assist_windspeed) {
return true;
}
return false;
}

29
APMrover2/sailboat.h
View File

@ -24,7 +24,10 @@ public:
static const struct AP_Param::GroupInfo var_info[];
// enabled
bool enabled() const { return enable != 0;}
bool sail_enabled() const { return enable > 0;}
// true if sailboat navigation (aka tacking) is enabled
bool nav_enabled() const;
// constructor
Sailboat();
@ -32,8 +35,15 @@ public:
// setup
void init();
// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
void update_mainsail(float desired_speed);
// 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
void get_pilot_desired_mainsail(float &mainsail_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);
// Velocity Made Good, this is the speed we are traveling towards the desired destination
float get_VMG() const;
@ -59,8 +69,18 @@ public:
// calculate the heading to sail on if we cant go upwind
float calc_heading(float desired_heading_cd);
// throttle state used with throttle enum
enum Sailboat_Throttle {
NEVER = 0,
ASSIST = 1,
FORCE_MOTOR = 2
} throttle_state;
private:
// Check if we should assist with throttle
bool throttle_assist() const;
// parameters
AP_Int8 enable;
AP_Float sail_angle_min;
@ -68,14 +88,17 @@ private:
AP_Float sail_angle_ideal;
AP_Float sail_heel_angle_max;
AP_Float sail_no_go;
AP_Float sail_assist_windspeed;
enum Sailboat_Tack {
TACK_PORT,
TACK_STARBOARD
};
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
float tack_heading_rad; // target heading in radians while tacking in either acro or autonomous modes
uint32_t auto_tack_request_ms; // system time user requested tack in autonomous modes
uint32_t auto_tack_start_ms; // system time when tack was started in autonomous mode
bool tack_assist; // true if we should use some throttle to assist tack
};

4
APMrover2/system.cpp
View File

@ -75,8 +75,6 @@ void Rover::init_ardupilot()
g2.windvane.init(serial_manager);
rover.g2.sailboat.init();
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
@ -155,6 +153,8 @@ void Rover::init_ardupilot()
// initialise rc channels
rc().init();
rover.g2.sailboat.init();
// disable safety if requested
BoardConfig.init_safety();