Rover: Add optical flow support

This commit is contained in:
Stephen Dade 2022-09-07 16:49:51 +10:00 committed by Randy Mackay
parent b7e0b1ad38
commit 6490436aca
8 changed files with 39 additions and 2 deletions

View File

@ -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,

View File

@ -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;

View File

@ -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} },

View File

@ -222,6 +222,7 @@ public:
k_param_notify,
k_param_button,
k_param_osd,
k_param_optflow,
k_param_logger = 253, // Logging Group

View File

@ -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),

View File

@ -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))

View File

@ -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;
}

View File

@ -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