From 6490436aca81aabad97fb677cb160b9ed3ff1b0b Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Wed, 7 Sep 2022 16:49:51 +1000 Subject: [PATCH] Rover: Add optical flow support --- Rover/GCS_Mavlink.cpp | 1 + Rover/GCS_Rover.cpp | 11 +++++++++++ Rover/Parameters.cpp | 8 +++++++- Rover/Parameters.h | 1 + Rover/Rover.cpp | 3 +++ Rover/defines.h | 2 +- Rover/ekf_check.cpp | 10 ++++++++++ Rover/system.cpp | 5 +++++ 8 files changed, 39 insertions(+), 2 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 261ba9c760..eafa969d1e 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -561,6 +561,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_SYSTEM_TIME, MSG_BATTERY_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, + MSG_OPTICAL_FLOW, MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, MSG_EKF_STATUS_REPORT, diff --git a/Rover/GCS_Rover.cpp b/Rover/GCS_Rover.cpp index 9ad82f7e32..3e72643d14 100644 --- a/Rover/GCS_Rover.cpp +++ b/Rover/GCS_Rover.cpp @@ -58,6 +58,17 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void) } #endif +#if AP_OPTICALFLOW_ENABLED + const AP_OpticalFlow *optflow = AP::opticalflow(); + if (optflow && optflow->enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } + if (optflow && optflow->healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + const RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder && rangefinder->num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 78a4056c52..becc0bf55d 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -19,7 +19,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: Bitmap of what log types to enable in on-board logger. This value is made up of the sum of each of the log types you want to be saved. On boards supporting microSD cards or other large block-storage devices it is usually best just to enable all basic log types by setting this to 65535. - // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Throttle,5:Navigation Tuning,7:IMU,8:Mission Commands,9:Battery Monitor,10:Rangefinder,11:Compass,12:Camera,13:Steering,14:RC Input-Output,19:Raw IMU,20:Video Stabilization + // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Throttle,5:Navigation Tuning,7:IMU,8:Mission Commands,9:Battery Monitor,10:Rangefinder,11:Compass,12:Camera,13:Steering,14:RC Input-Output,19:Raw IMU,20:Video Stabilization,21:Optical Flow // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -391,6 +391,12 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(osd, "OSD", AP_OSD), #endif +#if AP_OPTICALFLOW_ENABLED + // @Group: FLOW + // @Path: ../libraries/AP_OpticalFlow/AP_OpticalFlow.cpp + GOBJECT(optflow, "FLOW", AP_OpticalFlow), +#endif + // @Group: // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp { AP_PARAM_GROUP, "", Parameters::k_param_vehicle, (const void *)&rover, {group_info : AP_Vehicle::var_info} }, diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 7cc973416d..313803469f 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -222,6 +222,7 @@ public: k_param_notify, k_param_button, k_param_osd, + k_param_optflow, k_param_logger = 253, // Logging Group diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index e85d93f2b1..398ae33c57 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -73,6 +73,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(read_radio, 50, 200, 3), SCHED_TASK(ahrs_update, 400, 400, 6), SCHED_TASK(read_rangefinders, 50, 200, 9), +#if AP_OPTICALFLOW_ENABLED + SCHED_TASK_CLASS(AP_OpticalFlow, &rover.optflow, update, 200, 160, 11), +#endif SCHED_TASK(update_current_mode, 400, 200, 12), SCHED_TASK(set_servos, 400, 200, 15), SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300, 18), diff --git a/Rover/defines.h b/Rover/defines.h index 68aa99e6f0..5fd45c2a2c 100644 --- a/Rover/defines.h +++ b/Rover/defines.h @@ -43,7 +43,7 @@ enum LoggingParameters { // #define MASK_LOG_ARM_DISARM (1<<15) #define MASK_LOG_IMU_RAW (1UL<<19) #define MASK_LOG_VIDEO_STABILISATION (1UL<<20) - +#define MASK_LOG_OPTFLOW (1UL<<21) // for mavlink SET_POSITION_TARGET messages #define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1)) diff --git a/Rover/ekf_check.cpp b/Rover/ekf_check.cpp index 94bb8505da..7f6d04b637 100644 --- a/Rover/ekf_check.cpp +++ b/Rover/ekf_check.cpp @@ -108,6 +108,16 @@ bool Rover::ekf_over_threshold() over_thresh_count++; } + bool optflow_healthy = false; +#if AP_OPTICALFLOW_ENABLED + optflow_healthy = optflow.healthy(); +#endif + if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) { + over_thresh_count += 2; + } else if (vel_variance >= g.fs_ekf_thresh) { + over_thresh_count++; + } + if (over_thresh_count >= 2) { return true; } diff --git a/Rover/system.cpp b/Rover/system.cpp index 22dff8131a..17cbf65ed0 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -100,6 +100,11 @@ void Rover::init_ardupilot() g2.torqeedo.init(); #endif +#if AP_OPTICALFLOW_ENABLED + // initialise optical flow sensor + optflow.init(MASK_LOG_OPTFLOW); +#endif // AP_OPTICALFLOW_ENABLED + relay.init(); #if HAL_MOUNT_ENABLED