mirror of https://github.com/ArduPilot/ardupilot
AP_OpticalFlow: add and use AP_OPTICALFLOW_CALIBRATOR_ENABLED
This commit is contained in:
parent
f14aab29e5
commit
8416730a8c
|
@ -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)
|
||||
{
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue