Sub: create and use AP_OPTICALFLOW_ENABLED

This commit is contained in:
Peter Barker 2021-12-25 09:05:23 +11:00 committed by Andrew Tridgell
parent e55eea961f
commit f637facea2
6 changed files with 7 additions and 15 deletions

View File

@ -48,7 +48,7 @@ SCHED_TASK_CLASS arguments:
const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(fifty_hz_loop, 50, 75, 3),
SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200, 6),
#if OPTFLOW == ENABLED
#if AP_OPTICALFLOW_ENABLED
SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160, 9),
#endif
SCHED_TASK(update_batt_compass, 10, 120, 12),

View File

@ -56,7 +56,7 @@ void GCS_Sub::update_vehicle_sensor_status_flags()
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
#if OPTFLOW == ENABLED
#if AP_OPTICALFLOW_ENABLED
const OpticalFlow *optflow = AP::opticalflow();
if (optflow && optflow->enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;

View File

@ -599,7 +599,7 @@ const AP_Param::Info Sub::var_info[] = {
GOBJECT(terrain, "TERRAIN_", AP_Terrain),
#endif
#if OPTFLOW == ENABLED
#if AP_OPTICALFLOW_ENABLED
// @Group: FLOW
// @Path: ../libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
GOBJECT(optflow, "FLOW", OpticalFlow),

View File

@ -77,11 +77,9 @@
#include "AP_Arming_Sub.h"
#include "GCS_Sub.h"
// libraries which are dependent on #defines in defines.h and/or config.h
#if OPTFLOW == ENABLED
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
#endif
// libraries which are dependent on #defines in defines.h and/or config.h
#if RCMAP_ENABLED == ENABLED
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#endif
@ -167,7 +165,7 @@ private:
FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)};
// Optical flow sensor
#if OPTFLOW == ENABLED
#if AP_OPTICALFLOW_ENABLED
OpticalFlow optflow;
#endif

View File

@ -99,12 +99,6 @@
# define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#ifndef OPTFLOW
# define OPTFLOW DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// gripper
#ifndef GRIPPER_ENABLED

View File

@ -85,7 +85,7 @@ void Sub::init_ardupilot()
AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init();
#if OPTFLOW == ENABLED
#if AP_OPTICALFLOW_ENABLED
// initialise optical flow sensor
optflow.init(MASK_LOG_OPTFLOW);
#endif
@ -231,7 +231,7 @@ bool Sub::optflow_position_ok()
// return immediately if neither optflow nor visual odometry is enabled
bool enabled = false;
#if OPTFLOW == ENABLED
#if AP_OPTICALFLOW_ENABLED
if (optflow.enabled()) {
enabled = true;
}