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

Rover: make sailboat a class

This commit is contained in:
Peter Hall 2019-05-07 19:21:02 +01:00 committed by Randy Mackay
parent d5f3cf50e4
commit 2d9a0195ce
4 changed files with 219 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
APMrover2/Rover.h
View File

@ -93,6 +93,7 @@
#include "AP_MotorsUGV.h"
#include "mode.h"
#include "AP_Arming.h"
#include "sailboat.h"
// Configuration
#include "config.h"
#include "defines.h"
@ -132,6 +133,8 @@ public:
friend class RC_Channel_Rover;
friend class RC_Channels_Rover;
friend class Sailboat;
Rover(void);
// HAL::Callbacks implementation.
@ -347,18 +350,6 @@ private:
} cruise_learn_t;
cruise_learn_t cruise_learn;
// sailboat variables
enum Sailboat_Tack {
TACK_PORT,
TACK_STARBOARD
};
struct {
bool 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
} sailboat;
private:
// APMrover2.cpp
@ -444,17 +435,6 @@ private:
void radio_failsafe_check(uint16_t pwm);
bool trim_radio();
// sailboat.cpp
void sailboat_update_mainsail(float desired_speed);
float sailboat_get_VMG() const;
void sailboat_handle_tack_request_acro();
float sailboat_get_tack_heading_rad() const;
void sailboat_handle_tack_request_auto();
void sailboat_clear_tack();
bool sailboat_tacking() const;
bool sailboat_use_indirect_route(float desired_heading_cd) const;
float sailboat_calc_heading(float desired_heading_cd);
// sensors.cpp
void init_compass_location(void);
void update_compass(void);
@ -525,6 +505,9 @@ public:
// Simple mode
float simple_sin_yaw;
// sailboat enabled
bool get_sailboat_enable() { return g2.sailboat.enabled(); }
};
extern const AP_HAL::HAL& hal;

180
APMrover2/sailboat.cpp
View File

@ -20,95 +20,174 @@ To Do List
- add option to do proper tacks, ie tacking on flat spot in the waves, or only try once at a certain speed, or some better method than just changing the desired heading suddenly
*/
// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
void Rover::sailboat_update_mainsail(float desired_speed)
const AP_Param::GroupInfo Sailboat::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable Sailboat
// @Description: This enables Sailboat functionality
// @Values: 0:Disable,1:Enable
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("ENABLE", 1, Sailboat, enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: ANGLE_MIN
// @DisplayName: Sail min angle
// @Description: Mainsheet tight, angle between centerline and boom
// @Units: deg
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGLE_MIN", 2, Sailboat, sail_angle_min, 0),
// @Param: ANGLE_MAX
// @DisplayName: Sail max angle
// @Description: Mainsheet loose, angle between centerline and boom
// @Units: deg
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGLE_MAX", 3, Sailboat, sail_angle_max, 90),
// @Param: ANGLE_IDEAL
// @DisplayName: Sail ideal angle
// @Description: Ideal angle between sail and apparent wind
// @Units: deg
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGLE_IDEAL", 4, Sailboat, sail_angle_ideal, 25),
// @Param: HEEL_MAX
// @DisplayName: Sailing maximum heel angle
// @Description: When in auto sail trim modes the heel will be limited to this value using PID control
// @Units: deg
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("HEEL_MAX", 5, Sailboat, sail_heel_angle_max, 15),
// @Param: SAIL_NO_GO_ANGLE
// @DisplayName: Sailing no go zone angle
// @Description: The typical closest angle to the wind the vehicle will sail at. the vehicle will sail at this angle when going upwind
// @Units: deg
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO("NO_GO_ANGLE", 6, Sailboat, sail_no_go, 45),
AP_GROUPEND
};
/*
constructor
*/
Sailboat::Sailboat()
{
if (!g2.motors.has_sail()) {
AP_Param::setup_object_defaults(this, var_info);
}
void Sailboat::init()
{
// sailboat defaults
if (enabled()) {
rover.g2.crash_angle.set_default(0);
rover.g2.loit_type.set_default(1);
rover.g2.loit_radius.set_default(5);
rover.g2.wp_nav.set_default_overshoot(10);
}
}
// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
void Sailboat::update_mainsail(float desired_speed)
{
if (!enabled()) {
return;
}
// relax sail if desired speed is zero
if (!is_positive(desired_speed)) {
g2.motors.set_mainsail(100.0f);
rover.g2.motors.set_mainsail(100.0f);
return;
}
// + 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(g2.windvane.get_apparent_wind_direction_rad());
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 - g2.sail_angle_ideal;
float mainsail_angle = wind_dir_apparent -sail_angle_ideal;
// make sure between allowable range
mainsail_angle = constrain_float(mainsail_angle, g2.sail_angle_min, g2.sail_angle_max);
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, g2.sail_angle_min, g2.sail_angle_max);
float mainsail = linear_interpolate(0.0f, 100.0f, mainsail_angle,sail_angle_min,sail_angle_max);
// use PID controller to sheet out
const float pid_offset = g2.attitude_control.get_sail_out_from_heel(radians(g2.sail_heel_angle_max), G_Dt) * 100.0f;
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);
g2.motors.set_mainsail(mainsail);
rover.g2.motors.set_mainsail(mainsail);
}
// Velocity Made Good, this is the speed we are traveling towards the desired destination
// only for logging at this stage
// https://en.wikipedia.org/wiki/Velocity_made_good
float Rover::sailboat_get_VMG() const
float Sailboat::get_VMG() const
{
// return 0 if not heading towards waypoint
if (!control_mode->is_autopilot_mode()) {
if (!rover.control_mode->is_autopilot_mode()) {
return 0.0f;
}
float speed;
if (!g2.attitude_control.get_forward_speed(speed)) {
if (!rover.g2.attitude_control.get_forward_speed(speed)) {
return 0.0f;
}
return (speed * cosf(wrap_PI(radians(g2.wp_nav.wp_bearing_cd() * 0.01f) - ahrs.yaw)));
return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.yaw)));
}
// handle user initiated tack while in acro mode
void Rover::sailboat_handle_tack_request_acro()
void Sailboat::handle_tack_request_acro()
{
// set tacking heading target to the current angle relative to the true wind but on the new tack
sailboat.tacking = true;
sailboat.tack_heading_rad = wrap_2PI(ahrs.yaw + 2.0f * wrap_PI((g2.windvane.get_absolute_wind_direction_rad() - ahrs.yaw)));
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)));
}
// return target heading in radians when tacking (only used in acro)
float Rover::sailboat_get_tack_heading_rad() const
float Sailboat::get_tack_heading_rad() const
{
return sailboat.tack_heading_rad;
return tack_heading_rad;
}
// handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
void Rover::sailboat_handle_tack_request_auto()
void Sailboat::handle_tack_request_auto()
{
// record time of request for tack. This will be processed asynchronously by sailboat_calc_heading
sailboat.auto_tack_request_ms = AP_HAL::millis();
auto_tack_request_ms = AP_HAL::millis();
}
// clear tacking state variables
void Rover::sailboat_clear_tack()
void Sailboat::clear_tack()
{
sailboat.tacking = false;
sailboat.auto_tack_request_ms = 0;
currently_tacking = false;
auto_tack_request_ms = 0;
}
// returns true if boat is currently tacking
bool Rover::sailboat_tacking() const
bool Sailboat::tacking() const
{
return sailboat.tacking;
return enabled() && currently_tacking;
}
// returns true if sailboat should take a indirect navigation route to go upwind
// desired_heading should be in centi-degrees
bool Rover::sailboat_use_indirect_route(float desired_heading_cd) const
bool Sailboat::use_indirect_route(float desired_heading_cd) const
{
if (!g2.motors.has_sail()) {
if (!enabled()) {
return false;
}
@ -116,14 +195,14 @@ bool Rover::sailboat_use_indirect_route(float desired_heading_cd) const
const float desired_heading_rad = radians(desired_heading_cd * 0.01f);
// check if desired heading is in the no go zone, if it is we can't go direct
return fabsf(wrap_PI(g2.windvane.get_absolute_wind_direction_rad() - desired_heading_rad)) <= radians(g2.sail_no_go);
return fabsf(wrap_PI(rover.g2.windvane.get_absolute_wind_direction_rad() - desired_heading_rad)) <= radians(sail_no_go);
}
// if we can't sail on the desired heading then we should pick the best heading that we can sail on
// this function assumes the caller has already checked sailboat_use_indirect_route(desired_heading_cd) returned true
float Rover::sailboat_calc_heading(float desired_heading_cd)
float Sailboat::calc_heading(float desired_heading_cd)
{
if (!g2.motors.has_sail()) {
if (!enabled()) {
return desired_heading_cd;
}
@ -131,19 +210,19 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
// check for user requested tack
uint32_t now = AP_HAL::millis();
if (sailboat.auto_tack_request_ms != 0) {
if (auto_tack_request_ms != 0) {
// set should_tack flag is user requested tack within last 0.5 sec
should_tack = ((now - sailboat.auto_tack_request_ms) < 500);
sailboat.auto_tack_request_ms = 0;
should_tack = ((now - auto_tack_request_ms) < 500);
auto_tack_request_ms = 0;
}
// calculate left and right no go headings looking upwind
const float left_no_go_heading_rad = wrap_2PI(g2.windvane.get_absolute_wind_direction_rad() + radians(g2.sail_no_go));
const float right_no_go_heading_rad = wrap_2PI(g2.windvane.get_absolute_wind_direction_rad() - radians(g2.sail_no_go));
const float left_no_go_heading_rad = wrap_2PI(rover.g2.windvane.get_absolute_wind_direction_rad() + radians(sail_no_go));
const float right_no_go_heading_rad = wrap_2PI(rover.g2.windvane.get_absolute_wind_direction_rad() - radians(sail_no_go));
// calculate current tack, Port if heading is left of no-go, STBD if right of no-go
Sailboat_Tack current_tack;
if (is_negative(g2.windvane.get_apparent_wind_direction_rad())) {
if (is_negative(rover.g2.windvane.get_apparent_wind_direction_rad())) {
current_tack = TACK_PORT;
} else {
current_tack = TACK_STARBOARD;
@ -151,14 +230,15 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
// 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
if ((fabsf(g2.wp_nav.crosstrack_error()) >= g2.wp_nav.get_overshoot()) && !is_zero(g2.wp_nav.get_overshoot()) && !sailboat_tacking()) {
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()) && !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(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
if (is_positive(cross_track_error) && (current_tack == TACK_STARBOARD)) {
should_tack = true;
}
// if were on port tack we are traveling towards the right hand boundary
if (is_negative(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_PORT)) {
if (is_negative(cross_track_error) && (current_tack == TACK_PORT)) {
should_tack = true;
}
}
@ -169,28 +249,28 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
// calculate target heading for the new tack
switch (current_tack) {
case TACK_PORT:
sailboat.tack_heading_rad = right_no_go_heading_rad;
tack_heading_rad = right_no_go_heading_rad;
break;
case TACK_STARBOARD:
sailboat.tack_heading_rad = left_no_go_heading_rad;
tack_heading_rad = left_no_go_heading_rad;
break;
}
sailboat.tacking = true;
sailboat.auto_tack_start_ms = AP_HAL::millis();
currently_tacking = true;
auto_tack_start_ms = AP_HAL::millis();
}
// if we are tacking we maintain the target heading until the tack completes or times out
if (sailboat.tacking) {
if (currently_tacking) {
// check if we have reached target
if (fabsf(wrap_PI(sailboat.tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
sailboat_clear_tack();
} else if ((now - sailboat.auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
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");
sailboat_clear_tack();
clear_tack();
}
// return tack target heading
return degrees(sailboat.tack_heading_rad) * 100.0f;
return degrees(tack_heading_rad) * 100.0f;
}
// return closest possible heading to wind

81
APMrover2/sailboat.h Normal file
View File

@ -0,0 +1,81 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Rover Sailboat functionality
*/
class Sailboat
{
public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
// enabled
bool enabled() const { return enable != 0;}
// constructor
Sailboat();
// 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);
// 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() const;
// 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);
private:
// parameters
AP_Int8 enable;
AP_Float sail_angle_min;
AP_Float sail_angle_max;
AP_Float sail_angle_ideal;
AP_Float sail_heel_angle_max;
AP_Float sail_no_go;
enum Sailboat_Tack {
TACK_PORT,
TACK_STARBOARD
};
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
};

2
APMrover2/system.cpp
View File

@ -75,6 +75,8 @@ void Rover::init_ardupilot()
g2.windvane.init();
rover.g2.sailboat.init();
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();