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

Copter: remove redundant SurfaceTracking enumeration namespacing 

Also rename State to Surface to be more specific about what is being
set/tracked.
This commit is contained in:
Peter Barker 2019-10-25 22:31:31 +11:00 committed by Randy Mackay
parent 6b92f3dae9
commit b6920e14c8
3 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/Copter.h
View File

@ -289,17 +289,17 @@ private:
bool get_dist_for_logging(float &target_dist, float &actual_dist) const;
void invalidate_for_logging() { valid_for_logging = false; }
// surface tracking states
enum class SurfaceTrackingState {
SURFACE_TRACKING_DISABLED = 0,
SURFACE_TRACKING_GROUND = 1,
SURFACE_TRACKING_CEILING = 2
// surface tracking surface
enum class Surface {
NONE = 0,
GROUND = 1,
CEILING = 2
};
// set direction
void set_state(SurfaceTrackingState state);
// set surface to track
void set_surface(Surface new_surface);
private:
SurfaceTrackingState tracking_state = SurfaceTrackingState::SURFACE_TRACKING_GROUND;
Surface surface = Surface::GROUND;
float target_dist_cm; // desired distance in cm from ground or ceiling
uint32_t last_update_ms; // system time of last update to target_alt_cm
uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery

6
ArduCopter/RC_Channel.cpp
View File

@ -562,13 +562,13 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
case AUX_FUNC::SURFACE_TRACKING:
switch (ch_flag) {
case LOW:
copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_GROUND);
copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::GROUND);
break;
case MIDDLE:
copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_DISABLED);
copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::NONE);
break;
case HIGH:
copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_CEILING);
copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::CEILING);
break;
}
break;

30
ArduCopter/surface_tracking.cpp
View File

@ -7,9 +7,9 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
{
#if RANGEFINDER_ENABLED == ENABLED
// check tracking state and that range finders are healthy
if ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED) ||
((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
if ((surface == Surface::NONE) ||
((surface == Surface::GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
((surface == Surface::CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
return target_rate;
}
@ -17,8 +17,8 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
const float current_alt_error = copter.pos_control->get_alt_target() - copter.inertial_nav.get_altitude();
// init based on tracking direction/state
RangeFinderState &rf_state = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
const float dir = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? 1.0f : -1.0f;
RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
// reset target altitude if this controller has just been engaged
// target has been changed between upwards vs downwards
@ -40,7 +40,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
valid_for_logging = true;
// upward facing terrain following never gets closer than avoidance margin
if (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) {
if (surface == Surface::CEILING) {
const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f;
target_dist_cm = MAX(target_dist_cm, margin_cm);
}
@ -62,7 +62,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
{
// fail if we are not tracking downwards
if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
if (surface != Surface::GROUND) {
return false;
}
// check target has been updated recently
@ -77,7 +77,7 @@ bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
{
// fail if we are not tracking downwards
if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
if (surface != Surface::GROUND) {
return;
}
target_dist_cm = _target_alt_cm;
@ -86,31 +86,31 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const
{
if (!valid_for_logging || (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED)) {
if (!valid_for_logging || (surface == Surface::NONE)) {
return false;
}
target_dist = target_dist_cm * 0.01f;
actual_dist = ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
actual_dist = ((surface == Surface::GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
return true;
}
// set direction
void Copter::SurfaceTracking::set_state(SurfaceTrackingState state)
void Copter::SurfaceTracking::set_surface(Surface new_surface)
{
if (tracking_state == state) {
if (surface == new_surface) {
return;
}
// check we have a range finder in the correct direction
if ((state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
if ((new_surface == Surface::GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
AP_Notify::events.user_mode_change_failed = 1;
return;
}
if ((state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
if ((new_surface == Surface::CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
AP_Notify::events.user_mode_change_failed = 1;
return;
}
tracking_state = state;
surface = new_surface;
reset_target = true;
}