diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 94f34af4e8..8352e3d32b 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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 diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 37cc054174..6394ec886e 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -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; diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index b31c18fdcf..3a490298e7 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -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; }