AP_OpticalFlow: add and use AP_OPTICALFLOW_CALIBRATOR_ENABLED

This commit is contained in:
Peter Barker 2023-07-12 16:23:33 +10:00 committed by Peter Barker
parent f14aab29e5
commit 8416730a8c
4 changed files with 17 additions and 1 deletions

View File

@ -193,6 +193,7 @@ void AP_OpticalFlow::update(void)
// only healthy if the data is less than 0.5s old
_flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
#if AP_OPTICALFLOW_CALIBRATOR_ENABLED
// update calibrator and save resulting scaling
if (_calibrator != nullptr) {
if (_calibrator->update()) {
@ -207,6 +208,7 @@ void AP_OpticalFlow::update(void)
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FlowCal: FLOW_FXSCALER=%d, FLOW_FYSCALER=%d", (int)_flowScalerX, (int)_flowScalerY);
}
}
#endif
}
void AP_OpticalFlow::handle_msg(const mavlink_message_t &msg)
@ -235,6 +237,7 @@ void AP_OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
}
#endif //HAL_MSP_OPTICALFLOW_ENABLED
#if AP_OPTICALFLOW_CALIBRATOR_ENABLED
// start calibration
void AP_OpticalFlow::start_calibration()
{
@ -257,6 +260,7 @@ void AP_OpticalFlow::stop_calibration()
_calibrator->stop();
}
}
#endif
void AP_OpticalFlow::update_state(const OpticalFlow_state &state)
{

View File

@ -139,9 +139,10 @@ private:
void Log_Write_Optflow();
uint32_t _log_bit = -1; // bitmask bit which indicates if we should log. -1 means we always log
#if AP_OPTICALFLOW_CALIBRATOR_ENABLED
// calibrator
AP_OpticalFlow_Calibrator *_calibrator;
#endif
};
namespace AP {

View File

@ -13,7 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_OpticalFlow_config.h"
#if AP_OPTICALFLOW_CALIBRATOR_ENABLED
#include "AP_OpticalFlow_Calibrator.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
@ -329,3 +334,5 @@ void AP_OpticalFlow_Calibrator::log_sample(uint8_t axis, uint8_t sample_num, flo
(double)los_pred);
}
#endif // HAL_LOGGING_ENABLED
#endif // AP_OPTICALFLOW_CALIBRATOR_ENABLED

View File

@ -8,6 +8,10 @@
#define AP_OPTICALFLOW_ENABLED 1
#endif
#ifndef AP_OPTICALFLOW_CALIBRATOR_ENABLED
#define AP_OPTICALFLOW_CALIBRATOR_ENABLED AP_OPTICALFLOW_ENABLED
#endif
#ifndef AP_OPTICALFLOW_BACKEND_DEFAULT_ENABLED
#define AP_OPTICALFLOW_BACKEND_DEFAULT_ENABLED AP_OPTICALFLOW_ENABLED
#endif