Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Use enum class rather than typedef enum for states and hide details of these.

Browse Source
This commit is contained in:
Samuel Tabor 2020-04-05 12:38:19 +01:00 committed by Andrew Tridgell
parent d7ce3ff72e
commit b9daae062c
7 changed files with 43 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -265,7 +265,7 @@ int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const
float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
{
#if SOARING_ENABLED == ENABLED
if (plane.g2.soaring_controller.update_active_state()) {
if (plane.g2.soaring_controller.is_active()) {
return plane.g2.soaring_controller.get_vario_reading();
}
#endif

2
ArduPlane/altitude.cpp
View File

@ -41,7 +41,7 @@ void Plane::adjust_altitude_target()
} else if (landing.get_target_altitude_location(target_location)) {
set_target_altitude_location(target_location);
#if SOARING_ENABLED == ENABLED
} else if (g2.soaring_controller.active_state() && g2.soaring_controller.get_throttle_suppressed()) {
} else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
// Reset target alt to current alt, to prevent large altitude errors when gliding.
set_target_altitude_location(current_loc);
reset_offset_altitude();

2
ArduPlane/mode_loiter.cpp
View File

@ -10,7 +10,7 @@ bool ModeLoiter::_enter()
plane.loiter_angle_reset();
#if SOARING_ENABLED == ENABLED
if (plane.g2.soaring_controller.active_state()) {
if (plane.g2.soaring_controller.is_active()) {
plane.g2.soaring_controller.init_thermalling();
plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
}

2
ArduPlane/navigation.cpp
View File

@ -320,7 +320,7 @@ void Plane::update_fbwb_speed_height(void)
}
#if SOARING_ENABLED == ENABLED
if (g2.soaring_controller.active_state() && g2.soaring_controller.get_throttle_suppressed()) {
if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
// we're in soaring mode with throttle suppressed
set_target_altitude_current();;
}

6
ArduPlane/soaring.cpp
View File

@ -11,7 +11,9 @@ void Plane::update_soaring() {
// Check if soaring is active. Also sets throttle suppressed
// status on active state changes.
if (!g2.soaring_controller.update_active_state()) {
plane.g2.soaring_controller.update_active_state();
if (!g2.soaring_controller.is_active()) {
return;
}
@ -83,7 +85,7 @@ void Plane::update_soaring() {
const SoaringController::LoiterStatus loiterStatus = g2.soaring_controller.check_cruise_criteria(prev_wp/100, next_wp/100);
if (loiterStatus == SoaringController::LoiterStatus::THERMAL_GOOD_TO_KEEP_LOITERING) {
if (loiterStatus == SoaringController::LoiterStatus::GOOD_TO_KEEP_LOITERING) {
// Reset loiter angle, so that the loiter exit heading criteria
// only starts expanding when we're ready to exit.
plane.loiter.sum_cd = 0;

42
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -184,7 +184,7 @@ bool SoaringController::check_thermal_criteria()
{
ActiveStatus status = active_state();
return (status == SOARING_STATUS_AUTO_MODE_CHANGE
return (status == ActiveStatus::AUTO_MODE_CHANGE
&& ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6))
&& (_vario.filtered_reading - _vario.get_exp_thermalling_sink()) > thermal_vspeed
&& _vario.alt < alt_max
@ -196,38 +196,38 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2
{
ActiveStatus status = active_state();
if (status != SOARING_STATUS_AUTO_MODE_CHANGE) {
_cruise_criteria_msg_last = SOARING_DISABLED;
return SOARING_DISABLED;
if (status != ActiveStatus::AUTO_MODE_CHANGE) {
_cruise_criteria_msg_last = LoiterStatus::DISABLED;
return LoiterStatus::DISABLED;
}
LoiterStatus result = THERMAL_GOOD_TO_KEEP_LOITERING;
LoiterStatus result = LoiterStatus::GOOD_TO_KEEP_LOITERING;
const float alt = _vario.alt;
if (alt > alt_max) {
result = ALT_TOO_HIGH;
result = LoiterStatus::ALT_TOO_HIGH;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper alt = %dm", (int16_t)alt);
}
} else if (alt < alt_min) {
result = ALT_TOO_LOW;
result = LoiterStatus::ALT_TOO_LOW;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_ALERT, "Reached lower alt = %dm", (int16_t)alt);
}
} else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) {
const float mcCreadyAlt = McCready(alt);
if (_thermalability < mcCreadyAlt) {
result = THERMAL_WEAK;
result = LoiterStatus::THERMAL_WEAK;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: th %3.1fm/s alt %3.1fm Mc %3.1fm/s", (double)_thermalability, (double)alt, (double)mcCreadyAlt);
}
} else if (alt < (-_thermal_start_pos.z) || _vario.smoothed_climb_rate < 0.0) {
result = ALT_LOST;
result = LoiterStatus::ALT_LOST;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
}
} else if (check_drift(prev_wp, next_wp)) {
result = DRIFT_EXCEEDED;
result = LoiterStatus::DRIFT_EXCEEDED;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Drifted too far");
}
@ -288,7 +288,7 @@ void SoaringController::init_thermalling()
void SoaringController::init_cruising()
{
if (active_state()) {
if (active_state()>=ActiveStatus::MANUAL_MODE_CHANGE) {
_cruise_start_time_us = AP_HAL::micros64();
// Start glide. Will be updated on the next loop.
set_throttle_suppressed(true);
@ -359,43 +359,43 @@ float SoaringController::McCready(float alt)
SoaringController::ActiveStatus SoaringController::active_state() const
{
if (!soar_active) {
return SOARING_STATUS_DISABLED;
return ActiveStatus::DISABLED;
}
if (soar_active_ch <= 0) {
// no activation channel
return SOARING_STATUS_AUTO_MODE_CHANGE;
return ActiveStatus::AUTO_MODE_CHANGE;
}
uint16_t radio_in = RC_Channels::get_radio_in(soar_active_ch-1);
// active when above 1400, with auto mode changes when above 1700
if (radio_in >= 1700) {
return SOARING_STATUS_AUTO_MODE_CHANGE;
return ActiveStatus::AUTO_MODE_CHANGE;
} else if (radio_in >= 1400) {
return SOARING_STATUS_MANUAL_MODE_CHANGE;
return ActiveStatus::MANUAL_MODE_CHANGE;
}
return SOARING_STATUS_DISABLED;
return ActiveStatus::DISABLED;
}
SoaringController::ActiveStatus SoaringController::update_active_state()
void SoaringController::update_active_state()
{
ActiveStatus status = active_state();
bool state_changed = !(status == _last_update_status);
if (state_changed) {
switch (status) {
case SOARING_STATUS_DISABLED:
case ActiveStatus::DISABLED:
// It's not enabled, but was enabled on the last loop.
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Disabled.");
set_throttle_suppressed(false);
break;
case SOARING_STATUS_MANUAL_MODE_CHANGE:
case ActiveStatus::MANUAL_MODE_CHANGE:
// It's enabled, but wasn't on the last loop.
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes.");
set_throttle_suppressed(true);
break;
case SOARING_STATUS_AUTO_MODE_CHANGE:
case ActiveStatus::AUTO_MODE_CHANGE:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes.");
set_throttle_suppressed(true);
break;
@ -403,8 +403,6 @@ SoaringController::ActiveStatus SoaringController::update_active_state()
}
_last_update_status = status;
return status;
}

27
libraries/AP_Soaring/AP_Soaring.h
View File

@ -76,21 +76,21 @@ protected:
public:
SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
typedef enum LoiterStatus {
SOARING_DISABLED,
enum class LoiterStatus {
DISABLED,
ALT_TOO_HIGH,
ALT_TOO_LOW,
THERMAL_WEAK,
ALT_LOST,
DRIFT_EXCEEDED,
THERMAL_GOOD_TO_KEEP_LOITERING
} LoiterStatus;
GOOD_TO_KEEP_LOITERING
};
typedef enum ActiveStatus {
SOARING_STATUS_DISABLED,
SOARING_STATUS_MANUAL_MODE_CHANGE,
SOARING_STATUS_AUTO_MODE_CHANGE
} ActiveStatus;
enum class ActiveStatus {
DISABLED,
MANUAL_MODE_CHANGE,
AUTO_MODE_CHANGE
};
AP_Float max_radius;
@ -104,7 +104,6 @@ public:
void init_cruising();
void update_thermalling();
void update_cruising();
ActiveStatus active_state() const;
void set_throttle_suppressed(bool suppressed);
bool get_throttle_suppressed() const
@ -121,11 +120,15 @@ public:
bool check_drift(Vector2f prev_wp, Vector2f next_wp);
ActiveStatus update_active_state();
void update_active_state();
ActiveStatus _last_update_status;
bool is_active() const {return _last_update_status>=SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;};
private:
// slow down messages if they are the same. During loiter we could smap the same message. Only show new messages during loiters
LoiterStatus _cruise_criteria_msg_last;
ActiveStatus _last_update_status;
ActiveStatus active_state() const;
};