Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: init accepts update rate

Browse Source
This commit is contained in:
Randy Mackay 2018-09-21 16:01:30 +09:00
parent 7fbdaa3c65
commit d271dda8c5
2 changed files with 15 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -114,8 +114,9 @@ AC_PrecLand::AC_PrecLand()
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
// init - perform any required initialisation of backends // perform any required initialisation of landing controllers
void AC_PrecLand::init() // update_rate_hz should be the rate at which the update method will be called in hz
void AC_PrecLand::init(uint16_t update_rate_hz)
{ {
// exit immediately if init has already been run // exit immediately if init has already been run
if (_backend != nullptr) { if (_backend != nullptr) {
@ -126,8 +127,16 @@ void AC_PrecLand::init()
_backend = nullptr; _backend = nullptr;
_backend_state.healthy = false; _backend_state.healthy = false;
// create inertial history buffer, return on failure so backends are not created // create inertial history buffer
if (!init_inertial_buffer()) { // constrain lag parameter to be within bounds
_lag = constrain_float(_lag, 0.02f, 0.25f);
// calculate inertial buffer size from lag and minimum of main loop rate and update_rate_hz argument
const uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag * MIN(update_rate_hz, AP::scheduler().get_loop_rate_hz())), 1);
// instantiate ring buffer to hold inertial history, return on failure so no backends are created
_inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
if (_inertial_history == nullptr) {
return; return;
} }
@ -251,22 +260,6 @@ void AC_PrecLand::handle_msg(mavlink_message_t* msg)
// Private methods // Private methods
// //
// initialise inertial buffer
bool AC_PrecLand::init_inertial_buffer()
{
// constrain lag parameter to be within bounds
_lag = constrain_float(_lag, 0.02f, 0.25f);
// calculate inertial buffer size from delay length (loop runs at main loop rate)
uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag * AP::scheduler().get_loop_rate_hz()), 1);
// instantiate ring buffer to hold inertial history
_inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
// return success/failure
return (_inertial_history != nullptr);
}
void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid) void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
{ {
const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];

6
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -48,7 +48,8 @@ public:
}; };
// perform any required initialisation of landing controllers // perform any required initialisation of landing controllers
void init(); // update_rate_hz should be the rate at which the update method will be called in hz
void init(uint16_t update_rate_hz);
// returns true if precision landing is healthy // returns true if precision landing is healthy
bool healthy() const { return _backend_state.healthy; } bool healthy() const { return _backend_state.healthy; }
@ -101,9 +102,6 @@ private:
// returns enabled parameter as an behaviour // returns enabled parameter as an behaviour
enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); } enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
// initialise inertial buffer, return true on success
bool init_inertial_buffer();
// run target position estimator // run target position estimator
void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid); void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid);