diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index e88b9a5127..4d5db5d3d7 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -18,6 +18,7 @@ //#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands //#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space +//#define VISUAL_ODOMETRY_ENABLED DISABLED // disable visual odometry to save 2K of flash space //#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry //#define ADSB_ENABLED DISABLED // disable ADSB support //#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index ffa1e13cfc..047a76b649 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -96,6 +96,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(read_rangefinder, 20, 100), SCHED_TASK(update_proximity, 100, 50), SCHED_TASK(update_beacon, 400, 50), + SCHED_TASK(update_visual_odom, 400, 50), SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_throttle_hover,100, 90), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 11dc89d156..80ebea7380 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -91,6 +91,7 @@ #include // Heli specific pilot input handling library #include #include +#include // Configuration #include "defines.h" @@ -587,7 +588,12 @@ private: // last valid RC input time uint32_t last_radio_update_ms; - + +#if VISUAL_ODOMETRY_ENABLED == ENABLED + // last visual odometry update time + uint32_t visual_odom_last_update_ms; +#endif + // Top-level logic // setup the var_info table AP_Param param_loader; @@ -692,6 +698,8 @@ private: void stats_update(); void init_beacon(); void update_beacon(); + void init_visual_odom(); + void update_visual_odom(); void send_pid_tuning(mavlink_channel_t chan); void gcs_send_message(enum ap_message id); void gcs_send_mission_item_reached_message(uint16_t mission_index); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 3252f76018..ea32cf3510 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -2020,6 +2020,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) #endif break; + case MAVLINK_MSG_ID_VISION_POSITION_DELTA: +#if VISUAL_ODOMETRY_ENABLED == ENABLED + copter.g2.visual_odom.handle_msg(msg); +#endif + break; + default: handle_common_message(msg); break; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d7222a3180..47c273740a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -998,6 +998,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/RC_Channel/RC_Channels.cpp AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels), +#if VISUAL_ODOMETRY_ENABLED == ENABLED + // @Group: VISO + // @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp + AP_SUBGROUPINFO(visual_odom, "VISO", 18, ParametersG2, AP_VisualOdom), +#endif + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 89a7c53e27..a464ad9f03 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -536,6 +536,11 @@ public: // beacon (non-GPS positioning) library AP_Beacon beacon; +#if VISUAL_ODOMETRY_ENABLED == ENABLED + // Visual Odometry camera + AP_VisualOdom visual_odom; +#endif + #if PROXIMITY_ENABLED == ENABLED // proximity (aka object avoidance) library AP_Proximity proximity; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index c1df95d6a3..de18851e47 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -215,10 +215,13 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// OPTICAL_FLOW +// OPTICAL_FLOW & VISUAL ODOMETRY #ifndef OPTFLOW # define OPTFLOW ENABLED #endif +#ifndef VISUAL_ODOMETRY_ENABLED +# define VISUAL_ODOMETRY_ENABLED ENABLED +#endif ////////////////////////////////////////////////////////////////////////////// // Auto Tuning diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 703f5da68d..1472cc907e 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -297,6 +297,11 @@ void Copter::update_sensor_status_flags(void) if (precland.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } +#endif +#if VISUAL_ODOMETRY_ENABLED == ENABLED + if (g2.visual_odom.enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + } #endif if (ap.rc_receiver_present) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; @@ -382,7 +387,12 @@ void Copter::update_sensor_status_flags(void) } #endif #if PRECISION_LANDING == ENABLED - if (!precland.healthy()) { + if (precland.enabled() && !precland.healthy()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; + } +#endif +#if VISUAL_ODOMETRY_ENABLED == ENABLED + if (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; } #endif @@ -461,3 +471,34 @@ void Copter::update_beacon() { g2.beacon.update(); } + +// init visual odometry sensor +void Copter::init_visual_odom() +{ +#if VISUAL_ODOMETRY_ENABLED == ENABLED + g2.visual_odom.init(); +#endif +} + +// update visual odometry sensor +void Copter::update_visual_odom() +{ +#if VISUAL_ODOMETRY_ENABLED == ENABLED + // check for updates + if (g2.visual_odom.enabled() && (g2.visual_odom.get_last_update_ms() != visual_odom_last_update_ms)) { + visual_odom_last_update_ms = g2.visual_odom.get_last_update_ms(); + float time_delta_sec = g2.visual_odom.get_time_delta_usec() / 1000000.0f; + ahrs.writeBodyFrameOdom(g2.visual_odom.get_confidence(), + g2.visual_odom.get_position_delta(), + g2.visual_odom.get_angle_delta(), + time_delta_sec, + visual_odom_last_update_ms, + g2.visual_odom.get_pos_offset()); + // log sensor data + DataFlash.Log_Write_VisualOdom(time_delta_sec, + g2.visual_odom.get_angle_delta(), + g2.visual_odom.get_position_delta(), + g2.visual_odom.get_confidence()); + } +#endif +} diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index aa05f6a7c1..ab894be7ed 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -279,6 +279,9 @@ void Copter::init_ardupilot() // init beacons used for non-gps position estimation init_beacon(); + // init visual odometry + init_visual_odom(); + // initialise AP_RPM library rpm_sensor.init(); @@ -386,11 +389,27 @@ bool Copter::ekf_position_ok() // optflow_position_ok - returns true if optical flow based position estimate is ok bool Copter::optflow_position_ok() { -#if OPTFLOW != ENABLED +#if OPTFLOW != ENABLED && VISUAL_ODOMETRY_ENABLED != ENABLED return false; #else - // return immediately if optflow is not enabled or EKF not used - if (!optflow.enabled() || !ahrs.have_inertial_nav()) { + // return immediately if EKF not used + if (!ahrs.have_inertial_nav()) { + return false; + } + + // return immediately if neither optflow nor visual odometry is enabled + bool enabled = false; +#if OPTFLOW == ENABLED + if (optflow.enabled()) { + enabled = true; + } +#endif +#if VISUAL_ODOMETRY_ENABLED == ENABLED + if (g2.visual_odom.enabled()) { + enabled = true; + } +#endif + if (!enabled) { return false; }