mirror of https://github.com/ArduPilot/ardupilot
Copter: add configurable rangefinder filter
This commit is contained in:
parent
9056cd35ea
commit
1cafbe1e65
|
@ -1058,6 +1058,18 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0),
|
AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0),
|
||||||
|
|
||||||
|
#if RANGEFINDER_ENABLED == ENABLED
|
||||||
|
// @Param: RNGFND_FILT
|
||||||
|
// @DisplayName: Rangefinder filter
|
||||||
|
// @Description: Rangefinder filter to smooth distance. Set to zero to disable filtering
|
||||||
|
// @Units: Hz
|
||||||
|
// @Range: 0 50
|
||||||
|
// @Increment: 0.05
|
||||||
|
// @User: Standard
|
||||||
|
// @RebootRequired: True
|
||||||
|
AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT),
|
||||||
|
#endif
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -645,6 +645,10 @@ public:
|
||||||
|
|
||||||
AP_Int32 flight_options;
|
AP_Int32 flight_options;
|
||||||
|
|
||||||
|
#if RANGEFINDER_ENABLED == ENABLED
|
||||||
|
AP_Float rangefinder_filt;
|
||||||
|
#endif
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
extern const AP_Param::Info var_info[];
|
extern const AP_Param::Info var_info[];
|
||||||
|
|
|
@ -85,6 +85,10 @@
|
||||||
# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
|
# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef RANGEFINDER_FILT_DEFAULT
|
||||||
|
# define RANGEFINDER_FILT_DEFAULT 0.25f // filter for rangefinder distance
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef SURFACE_TRACKING_VELZ_MAX
|
#ifndef SURFACE_TRACKING_VELZ_MAX
|
||||||
# define SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
|
# define SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
|
||||||
#endif
|
#endif
|
||||||
|
@ -93,10 +97,6 @@
|
||||||
# define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
|
# define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef RANGEFINDER_WPNAV_FILT_HZ
|
|
||||||
# define RANGEFINDER_WPNAV_FILT_HZ 0.5f // filter frequency for rangefinder altitude provided to waypoint navigation class
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
|
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
|
||||||
# define RANGEFINDER_TILT_CORRECTION ENABLED
|
# define RANGEFINDER_TILT_CORRECTION ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -15,11 +15,11 @@ void Copter::init_rangefinder(void)
|
||||||
#if RANGEFINDER_ENABLED == ENABLED
|
#if RANGEFINDER_ENABLED == ENABLED
|
||||||
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
|
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
|
||||||
rangefinder.init(ROTATION_PITCH_270);
|
rangefinder.init(ROTATION_PITCH_270);
|
||||||
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
|
rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
|
||||||
rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
|
rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
|
||||||
|
|
||||||
// upward facing range finder
|
// upward facing range finder
|
||||||
rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
|
rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
|
||||||
rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
|
rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue