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

Rover: move sailboat vars into struct 

also fix airspeed library's parameter description
also log NaN when we do not have wind direction or speed estimates
also send timeout message to GCS when taking takes too long
This commit is contained in:
Randy Mackay 2018-11-01 14:28:45 +09:00
parent 4dbff6af7e
commit 2db6b7c7cb
4 changed files with 45 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/Log.cpp
View File

@ -167,10 +167,10 @@ void Rover::Log_Write_Sail()
} }
// get wind direction // get wind direction
float wind_dir_abs = 0.0f; float wind_dir_abs = DataFlash.quiet_nanf();
float wind_dir_rel = 0.0f; float wind_dir_rel = DataFlash.quiet_nanf();
float wind_speed_true = 0.0f; float wind_speed_true = DataFlash.quiet_nanf();
float wind_speed_apparent = 0.0f; float wind_speed_apparent = DataFlash.quiet_nanf();
if (rover.g2.windvane.enabled()) { if (rover.g2.windvane.enabled()) {
wind_dir_abs = degrees(g2.windvane.get_absolute_wind_direction_rad()); wind_dir_abs = degrees(g2.windvane.get_absolute_wind_direction_rad());
wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad()); wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad());

2
APMrover2/Parameters.cpp
View File

@ -658,7 +658,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("SAIL_NO_GO_ANGLE", 36, ParametersG2, sail_no_go, 45), AP_GROUPINFO("SAIL_NO_GO_ANGLE", 36, ParametersG2, sail_no_go, 45),
// @Group: ARSPD // @Group: ARSPD
// @Path: ../libraries/AP_WindVane/AP_WindVane.cpp // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
AP_SUBGROUPINFO(airspeed, "ARSPD", 37, ParametersG2, AP_Airspeed), AP_SUBGROUPINFO(airspeed, "ARSPD", 37, ParametersG2, AP_Airspeed),
AP_GROUPEND AP_GROUPEND

14
APMrover2/Rover.h
View File

@ -389,13 +389,15 @@ private:
// sailboat variables // sailboat variables
enum Sailboat_Tack { enum Sailboat_Tack {
Tack_Port, TACK_PORT,
Tack_STBD TACK_STARBOARD
}; };
bool _sailboat_tacking; // true when sailboat is in the process of tacking to a new heading struct {
float _sailboat_tack_heading_rad; // target heading in radians while tacking in either acro or autonomous modes bool tacking; // true when sailboat is in the process of tacking to a new heading
uint32_t _sailboat_auto_tack_request_ms;// system time user requested tack in autonomous modes float tack_heading_rad; // target heading in radians while tacking in either acro or autonomous modes
uint32_t _sailboat_auto_tack_start_ms; // system time when tack was started in autonomous mode 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: private:

61
APMrover2/sailboat.cpp
View File

@ -74,34 +74,34 @@ float Rover::sailboat_get_VMG() const
void Rover::sailboat_handle_tack_request_acro() 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 // set tacking heading target to the current angle relative to the true wind but on the new tack
_sailboat_tacking = true; sailboat.tacking = true;
_sailboat_tack_heading_rad = wrap_2PI(ahrs.yaw + 2.0f * wrap_PI((g2.windvane.get_absolute_wind_direction_rad() - ahrs.yaw))); 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) // return target heading in radians when tacking (only used in acro)
float Rover::sailboat_get_tack_heading_rad() const float Rover::sailboat_get_tack_heading_rad() const
{ {
return _sailboat_tack_heading_rad; return sailboat.tack_heading_rad;
} }
// handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL) // handle user initiated tack while in autonomous modes (Auto, Guided, RTL, SmartRTL, etc)
void Rover::sailboat_handle_tack_request_auto() void Rover::sailboat_handle_tack_request_auto()
{ {
// record time of request for tack. This will be processed asynchronously by sailboat_calc_heading // record time of request for tack. This will be processed asynchronously by sailboat_calc_heading
_sailboat_auto_tack_request_ms = AP_HAL::millis(); sailboat.auto_tack_request_ms = AP_HAL::millis();
} }
// clear tacking state variables // clear tacking state variables
void Rover::sailboat_clear_tack() void Rover::sailboat_clear_tack()
{ {
_sailboat_tacking = false; sailboat.tacking = false;
_sailboat_auto_tack_request_ms = 0; sailboat.auto_tack_request_ms = 0;
} }
// returns true if boat is currently tacking // returns true if boat is currently tacking
bool Rover::sailboat_tacking() const bool Rover::sailboat_tacking() const
{ {
return _sailboat_tacking; return sailboat.tacking;
} }
// returns true if sailboat should take a indirect navigation route to go upwind // returns true if sailboat should take a indirect navigation route to go upwind
@ -131,10 +131,10 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
// check for user requested tack // check for user requested tack
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (_sailboat_auto_tack_request_ms != 0) { if (sailboat.auto_tack_request_ms != 0) {
// set should_tack flag is user requested tack within last 0.5 sec // set should_tack flag is user requested tack within last 0.5 sec
should_tack = ((now - _sailboat_auto_tack_request_ms) < 500); should_tack = ((now - sailboat.auto_tack_request_ms) < 500);
_sailboat_auto_tack_request_ms = 0; sailboat.auto_tack_request_ms = 0;
} }
// calculate left and right no go headings looking upwind // calculate left and right no go headings looking upwind
@ -144,9 +144,9 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
// calculate current tack, Port if heading is left of no-go, STBD if right of no-go // calculate current tack, Port if heading is left of no-go, STBD if right of no-go
Sailboat_Tack current_tack; Sailboat_Tack current_tack;
if (is_negative(g2.windvane.get_apparent_wind_direction_rad())) { if (is_negative(g2.windvane.get_apparent_wind_direction_rad())) {
current_tack = Tack_Port; current_tack = TACK_PORT;
} else { } else {
current_tack = Tack_STBD; current_tack = TACK_STARBOARD;
} }
// trigger tack if cross track error larger than waypoint_overshoot parameter // trigger tack if cross track error larger than waypoint_overshoot parameter
@ -154,11 +154,11 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
if ((fabsf(rover.nav_controller->crosstrack_error()) >= g.waypoint_overshoot) && !is_zero(g.waypoint_overshoot) && !sailboat_tacking()) { 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 // 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 were on starboard tack we are traveling towards the left hand boundary
if (is_positive(rover.nav_controller->crosstrack_error()) && (current_tack == Tack_STBD)) { if (is_positive(rover.nav_controller->crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
should_tack = true; should_tack = true;
} }
// if were on port tack we are traveling towards the right hand boundary // 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)) { if (is_negative(rover.nav_controller->crosstrack_error()) && (current_tack == TACK_PORT)) {
should_tack = true; should_tack = true;
} }
} }
@ -168,30 +168,33 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking"); gcs().send_text(MAV_SEVERITY_INFO, "Sailboat: Tacking");
// calculate target heading for the new tack // calculate target heading for the new tack
switch (current_tack) { switch (current_tack) {
case Tack_Port: case TACK_PORT:
_sailboat_tack_heading_rad = right_no_go_heading_rad; sailboat.tack_heading_rad = right_no_go_heading_rad;
break; break;
case Tack_STBD: case TACK_STARBOARD:
_sailboat_tack_heading_rad = left_no_go_heading_rad; sailboat.tack_heading_rad = left_no_go_heading_rad;
break; break;
} }
_sailboat_tacking = true; sailboat.tacking = true;
_sailboat_auto_tack_start_ms = AP_HAL::millis(); sailboat.auto_tack_start_ms = AP_HAL::millis();
} }
// if were are tacking we maintain the target heading until the tack completes or timesout // if we are tacking we maintain the target heading until the tack completes or times out
if (_sailboat_tacking) { if (sailboat.tacking) {
// if we have reached target heading or timed out stop tacking on the next iteration // check if we have reached target
if (((now - _sailboat_auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) || if (fabsf(wrap_PI(sailboat.tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
(fabsf(wrap_PI(_sailboat_tack_heading_rad - ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG))) { sailboat_clear_tack();
_sailboat_tacking = false; } else if ((now - sailboat.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();
} }
// return tack target heading // return tack target heading
return degrees(_sailboat_tack_heading_rad) * 100.0f; return degrees(sailboat.tack_heading_rad) * 100.0f;
} }
// return closest possible heading to wind // return closest possible heading to wind
if (current_tack == Tack_Port) { if (current_tack == TACK_PORT) {
return degrees(left_no_go_heading_rad) * 100.0f; return degrees(left_no_go_heading_rad) * 100.0f;
} else { } else {
return degrees(right_no_go_heading_rad) * 100.0f; return degrees(right_no_go_heading_rad) * 100.0f;