Copter: move visual odometry update function into AP_VisualOdom

ArduCopter/ArduCopter.cpp

@@ -106,7 +106,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50),
 #endif
 #if VISUAL_ODOMETRY_ENABLED == ENABLED
-    SCHED_TASK(update_visual_odom,   400,     50),
+    SCHED_TASK_CLASS(AP_VisualOdom,       &copter.g2.visual_odom,        update,         400,  50),
 #endif
     SCHED_TASK(update_altitude,       10,    100),
     SCHED_TASK(run_nav_updates,       50,    100),

ArduCopter/Copter.h

@@ -549,11 +549,6 @@ private:
     // last esc calibration notification update
     uint32_t esc_calibration_notify_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;
@@ -837,7 +832,6 @@ private:
     void update_proximity();
     void update_sensor_status_flags(void);
     void init_visual_odom();
-    void update_visual_odom();
     void winch_init();
     void winch_update();
 

ArduCopter/sensors.cpp

@@ -403,29 +403,6 @@ void Copter::init_visual_odom()
 #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
-        logger.Write_VisualOdom(time_delta_sec,
-                                       g2.visual_odom.get_angle_delta(),
-                                       g2.visual_odom.get_position_delta(),
-                                       g2.visual_odom.get_confidence());
-    }
-#endif
-}
-
 // winch and wheel encoder initialisation
 void Copter::winch_init()
 {