Rover: add dedicated sailboat crosstrack and loiter params

This commit is contained in:
Peter Hall 2019-09-07 23:17:09 +01:00 committed by Andrew Tridgell
parent a244836f55
commit 6e78ef1dc3
3 changed files with 37 additions and 8 deletions

View File

@ -24,14 +24,22 @@ void ModeLoiter::update()
// get distance (in meters) to destination
_distance_to_destination = rover.current_loc.get_distance(_destination);
const float loiter_radius = rover.g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius;
// if within loiter radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g2.loit_radius) {
if (_distance_to_destination <= loiter_radius) {
// sailboats should not stop unless motoring
const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
_desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
// if we have a sail but not trying to use it then point into the wind
if (!rover.g2.sailboat.tack_enabled() && rover.g2.sailboat.sail_enabled()) {
_desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f;
}
} else {
// P controller with hard-coded gain to convert distance to desired speed
_desired_speed = MIN((_distance_to_destination - g2.loit_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
_desired_speed = MIN((_distance_to_destination - loiter_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
// calculate bearing to destination
_desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);

View File

@ -86,6 +86,24 @@ const AP_Param::GroupInfo Sailboat::var_info[] = {
// @User: Standard
AP_GROUPINFO("WNDSPD_MIN", 7, Sailboat, sail_windspeed_min, 0),
// @Param: XTRACK_MAX
// @DisplayName: Sailing vehicle max cross track error
// @Description: The sail boat will tack when it reaches this cross track error, defines a corridor of 2 times this value wide, 0 disables
// @Units: m
// @Range: 5 25
// @Increment: 1
// @User: Standard
AP_GROUPINFO("XTRACK_MAX", 8, Sailboat, xtrack_max, 10),
// @Param: LOIT_RADIUS
// @DisplayName: Loiter radius
// @Description: When in sailing modes the vehicle will keep moving within this loiter radius
// @Units: m
// @Range: 0 20
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LOIT_RADIUS", 9, Sailboat, loit_radius, 5),
AP_GROUPEND
};
@ -129,8 +147,6 @@ void Sailboat::init()
if (tack_enabled()) {
rover.g2.loit_type.set_default(1);
rover.g2.loit_radius.set_default(5);
rover.g2.wp_nav.set_default_overshoot(10);
}
// initialise motor state to USE_MOTOR_ASSIST
@ -361,10 +377,10 @@ float Sailboat::calc_heading(float desired_heading_cd)
tack_request_ms = 0;
}
// trigger tack if cross track error larger than waypoint_overshoot parameter
// this effectively defines a 'corridor' of width 2*waypoint_overshoot that the boat will stay within
// trigger tack if cross track error larger than xtrack_max parameter
// this effectively defines a 'corridor' of width 2*xtrack_max that the boat will stay within
const float cross_track_error = rover.g2.wp_nav.crosstrack_error();
if ((fabsf(cross_track_error) >= rover.g2.wp_nav.get_overshoot()) && !is_zero(rover.g2.wp_nav.get_overshoot()) && !should_tack && !currently_tacking) {
if ((fabsf(cross_track_error) >= xtrack_max) && !is_zero(xtrack_max) && !should_tack && !currently_tacking) {
// make sure the new tack will reduce the cross track error
// if were on starboard tack we are traveling towards the left hand boundary
if (is_positive(cross_track_error) && (current_tack == AP_WindVane::Sailboat_Tack::TACK_STARBOARD)) {

View File

@ -80,6 +80,9 @@ public:
// 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:
// true if motor is on to assist with slow tack
@ -96,6 +99,8 @@ private:
AP_Float sail_heel_angle_max;
AP_Float sail_no_go;
AP_Float sail_windspeed_min;
AP_Float xtrack_max;
AP_Float loit_radius;
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