Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: add sailboat tacking

Browse Source
This commit is contained in:
IamPete1 2018-09-14 16:09:07 +09:00 committed by Randy Mackay
parent bc1cf6a87a
commit 0dddc2eafe
13 changed files with 264 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
APMrover2/APMrover2.cpp
View File

@ -96,6 +96,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 200),
#endif
SCHED_TASK(read_airspeed, 10, 100),
};
void Rover::read_mode_switch()

4
APMrover2/GCS_Mavlink.cpp
View File

@ -289,8 +289,8 @@ void Rover::send_wind(mavlink_channel_t chan)
// send wind
mavlink_msg_wind_send(
chan,
degrees(rover.g2.windvane.get_apparent_wind_direction_rad()),
0,
wrap_360(degrees(g2.windvane.get_absolute_wind_direction_rad())),
g2.windvane.get_true_wind_speed(),
0);
}

13
APMrover2/Log.cpp
View File

@ -167,14 +167,23 @@ void Rover::Log_Write_Sail()
}
// get wind direction
float wind_dir_abs = 0.0f;
float wind_dir_rel = 0.0f;
float wind_speed_true = 0.0f;
float wind_speed_apparent = 0.0f;
if (rover.g2.windvane.enabled()) {
wind_dir_abs = degrees(g2.windvane.get_absolute_wind_direction_rad());
wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad());
wind_speed_true = g2.windvane.get_true_wind_speed();
wind_speed_apparent = g2.windvane.get_apparent_wind_speed();
}
DataFlash.Log_Write("SAIL", "TimeUS,WindDirRel,SailOut,VMG",
"sh%n", "F000", "Qfff",
DataFlash.Log_Write("SAIL", "TimeUS,WindDirAbs,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG",
"shhnn%n", "F000000", "Qffffff",
AP_HAL::micros64(),
(double)wind_dir_abs,
(double)wind_dir_rel,
(double)wind_speed_true,
(double)wind_speed_apparent,
(double)g2.motors.get_mainsail(),
(double)sailboat_get_VMG());
}

17
APMrover2/Parameters.cpp
View File

@ -647,6 +647,20 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Increment: 1
// @User: Standard
AP_GROUPINFO("SAIL_HEEL_MAX", 35, ParametersG2, 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("SAIL_NO_GO_ANGLE", 36, ParametersG2, sail_no_go, 45),
// @Group: ARSPD
// @Path: ../libraries/AP_WindVane/AP_WindVane.cpp
AP_SUBGROUPINFO(airspeed, "ARSPD", 37, ParametersG2, AP_Airspeed),
AP_GROUPEND
};
@ -679,7 +693,8 @@ ParametersG2::ParametersG2(void)
avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
follow(),
rally(rover.ahrs),
windvane()
windvane(),
airspeed()
{
AP_Param::setup_object_defaults(this, var_info);
}

4
APMrover2/Parameters.h
View File

@ -377,9 +377,13 @@ public:
AP_Float sail_angle_max;
AP_Float sail_angle_ideal;
AP_Float sail_heel_angle_max;
AP_Float sail_no_go;
// windvane
AP_WindVane windvane;
// Airspeed
AP_Airspeed airspeed;
};
extern const AP_Param::Info var_info[];

8
APMrover2/RC_Channel.cpp
View File

@ -90,6 +90,7 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const aux_s
case GUIDED:
case LOITER:
case FOLLOW:
case SAILBOAT_TACK:
break;
default:
RC_Channel::init_aux_function(ch_option, ch_flag);
@ -234,6 +235,13 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
do_aux_function_change_mode(rover.mode_simple, ch_flag);
break;
// trigger sailboat tack
case SAILBOAT_TACK:
if (ch_flag == HIGH) {
rover.control_mode->handle_tack_request();
}
break;
default:
RC_Channel::do_aux_function(ch_option, ch_flag);
break;

18
APMrover2/Rover.h
View File

@ -387,6 +387,16 @@ private:
LowPassFilterFloat throttle_filt = LowPassFilterFloat(2.0f);
} cruise_learn;
// sailboat variables
enum Sailboat_Tack {
Tack_Port,
Tack_STBD
};
bool _sailboat_tacking; // true when sailboat is in the process of tacking to a new heading
float _sailboat_tack_heading_rad; // target heading in radians while tacking in either acro or autonomous modes
uint32_t _sailboat_auto_tack_request_ms;// system time user requested tack in autonomous modes
uint32_t _sailboat_auto_tack_start_ms; // system time when tack was started in autonomous mode
private:
// APMrover2.cpp
@ -508,6 +518,13 @@ private:
// 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(void);
@ -521,6 +538,7 @@ private:
void accel_cal_update(void);
void read_rangefinders(void);
void init_proximity();
void read_airspeed();
void update_sensor_status_flags(void);
// Steering.cpp

18
APMrover2/mode.cpp
View File

@ -47,6 +47,9 @@ bool Mode::enter()
// initialisation common to all modes
if (ret) {
set_reversed(false);
// clear sailboat tacking flags
rover.sailboat_clear_tack();
}
return ret;
@ -254,6 +257,15 @@ void Mode::set_reversed(bool value)
_reversed = value;
}
// handle tacking request (from auxiliary switch) in sailboats
void Mode::handle_tack_request()
{
// autopilot modes handle tacking
if (is_autopilot_mode()) {
rover.sailboat_handle_tack_request_auto();
}
}
void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
{
// add in speed nudging
@ -435,7 +447,11 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct
}
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
if (rover.use_pivot_steering(_yaw_error_cd)) {
if (rover.sailboat_use_indirect_route(desired_heading)) {
// sailboats use heading controller when tacking upwind
desired_heading = rover.sailboat_calc_heading(desired_heading);
calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
} else if (rover.use_pivot_steering(_yaw_error_cd)) {
// for pivot turns use heading controller
calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
} else {

6
APMrover2/mode.h
View File

@ -116,6 +116,9 @@ public:
// execute the mission in reverse (i.e. backing up)
void set_reversed(bool value);
// handle tacking request (from auxiliary switch) in sailboats
virtual void handle_tack_request();
protected:
// subclasses override this to perform checks before entering the mode
@ -222,6 +225,9 @@ public:
// acro mode requires a velocity estimate for non skid-steer rovers
bool requires_position() const override { return false; }
bool requires_velocity() const override;
// sailboats in acro mode support user manually initiating tacking from transmitter
void handle_tack_request() override;
};

36
APMrover2/mode_acro.cpp
View File

@ -18,14 +18,30 @@ void ModeAcro::update()
calc_throttle(desired_speed, false, true);
}
// convert pilot steering input to desired turn rate in radians/sec
const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
float steering_out;
// run steering turn rate controller and throttle controller
const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
// handle sailboats
if (!is_zero(desired_steering)) {
// steering input return control to user
rover.sailboat_clear_tack();
}
if (g2.motors.has_sail() && rover.sailboat_tacking()) {
// call heading controller during tacking
steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
g2.pivot_turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
} else {
// convert pilot steering input to desired turn rate in radians/sec
const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
// run steering turn rate controller and throttle controller
steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
}
g2.motors.set_steering(steering_out * 4500.0f);
}
@ -34,3 +50,9 @@ bool ModeAcro::requires_velocity() const
{
return g2.motors.have_skid_steering()? false: true;
}
// sailboats in acro mode support user manually initiating tacking from transmitter
void ModeAcro::handle_tack_request()
{
rover.sailboat_handle_tack_request_acro();
}

130
APMrover2/sailboat.cpp
View File

@ -1,5 +1,7 @@
#include "Rover.h"
#define SAILBOAT_AUTO_TACKING_TIMEOUT_MS 50000 // tacks in auto mode timeout if not successfully completed within this many milliseconds
#define SAILBOAT_TACKING_ACCURACY_DEG 10 // tack is considered complete when vehicle is within this many degrees of target tack angle
/*
To Do List
- Improve tacking in light winds and bearing away in strong wings
@ -67,3 +69,131 @@ float Rover::sailboat_get_VMG() const
}
return (speed * cosf(wrap_PI(radians(nav_controller->target_bearing_cd()) - ahrs.yaw)));
}
// handle user initiated tack while in acro mode
void Rover::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)));
}
// return target heading in radians when tacking (only used in acro)
float Rover::sailboat_get_tack_heading_rad() const
{
return _sailboat_tack_heading_rad;
}
// handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL)
void Rover::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();
}
// clear tacking state variables
void Rover::sailboat_clear_tack()
{
_sailboat_tacking = false;
_sailboat_auto_tack_request_ms = 0;
}
// returns true if boat is currently tacking
bool Rover::sailboat_tacking() const
{
return _sailboat_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
{
if (!g2.motors.has_sail()) {
return false;
}
// convert desired heading to radians
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);
}
// 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)
{
if (!g2.motors.has_sail()) {
return desired_heading_cd;
}
bool should_tack = false;
// check for user requested tack
uint32_t now = AP_HAL::millis();
if (_sailboat_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;
}
// 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));
// 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())) {
current_tack = Tack_Port;
} else {
current_tack = Tack_STBD;
}
// 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(rover.nav_controller->crosstrack_error()) >= g.waypoint_overshoot) && !is_zero(g.waypoint_overshoot) && !sailboat_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(rover.nav_controller->crosstrack_error()) && (current_tack == Tack_STBD)) {
should_tack = true;
}
// if were on port tack we are traveling towards the right hand boundary
if (is_negative(rover.nav_controller->crosstrack_error()) && (current_tack == Tack_Port)) {
should_tack = true;
}
}
// if tack triggered, calculate target heading
if (should_tack) {
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking");
// calculate target heading for the new tack
switch (current_tack) {
case Tack_Port:
_sailboat_tack_heading_rad = right_no_go_heading_rad;
break;
case Tack_STBD:
_sailboat_tack_heading_rad = left_no_go_heading_rad;
break;
}
_sailboat_tacking = true;
_sailboat_auto_tack_start_ms = AP_HAL::millis();
}
// if were are tacking we maintain the target heading until the tack completes or timesout
if (_sailboat_tacking) {
// if we have reached target heading or timed out stop tacking on the next iteration
if (((now - _sailboat_auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) ||
(fabsf(wrap_PI(_sailboat_tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG))) {
_sailboat_tacking = false;
}
// return tack target heading
return degrees(_sailboat_tack_heading_rad) * 100.0f;
}
// return closest possible heading to wind
if (current_tack == Tack_Port) {
return degrees(left_no_go_heading_rad) * 100.0f;
} else {
return degrees(right_no_go_heading_rad) * 100.0f;
}
}

20
APMrover2/sensors.cpp
View File

@ -241,6 +241,26 @@ void Rover::init_proximity(void)
g2.proximity.set_rangefinder(&rangefinder);
}
/*
ask airspeed sensor for a new value, duplicated from plane
*/
void Rover::read_airspeed(void)
{
if (g2.airspeed.enabled()) {
g2.airspeed.read();
if (should_log(MASK_LOG_IMU)) {
DataFlash.Log_Write_Airspeed(g2.airspeed);
}
// supply a new temperature to the barometer from the digital
// airspeed sensor if we can
float temperature;
if (g2.airspeed.get_temperature(temperature)) {
barometer.set_external_temperature(temperature);
}
}
}
// update error mask of sensors and subsystems. The mask
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
// then it indicates that the sensor or subsystem is present but

2
APMrover2/system.cpp
View File

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