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

Copter: add configurable rangefinder filter

This commit is contained in:
Randy Mackay 2021-06-14 12:38:37 +09:00 committed by Andrew Tridgell
parent 9056cd35ea
commit 1cafbe1e65
4 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/Parameters.cpp
View File

@ -1058,6 +1058,18 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
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
};

4
ArduCopter/Parameters.h
View File

@ -645,6 +645,10 @@ public:
AP_Int32 flight_options;
#if RANGEFINDER_ENABLED == ENABLED
AP_Float rangefinder_filt;
#endif
};
extern const AP_Param::Info var_info[];

8
ArduCopter/config.h
View File

@ -85,6 +85,10 @@
# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
#endif
#ifndef RANGEFINDER_FILT_DEFAULT
# define RANGEFINDER_FILT_DEFAULT 0.25f // filter for rangefinder distance
#endif
#ifndef SURFACE_TRACKING_VELZ_MAX
# define SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
#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
#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
# define RANGEFINDER_TILT_CORRECTION ENABLED
#endif

4
ArduCopter/sensors.cpp
View File

@ -15,11 +15,11 @@ void Copter::init_rangefinder(void)
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
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);
// 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);
#endif
}