Rover: move visual odometry update function into AP_VisualOdom

APMrover2/APMrover2.cpp

@@ -54,7 +54,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Beacon,           &rover.g2.beacon,        update,         50,  200),
     SCHED_TASK_CLASS(AP_Proximity,        &rover.g2.proximity,     update,         50,  200),
     SCHED_TASK_CLASS(AP_WindVane,         &rover.g2.windvane,      update,         20,  100),
-    SCHED_TASK(update_visual_odom,     50,    200),
+#if VISUAL_ODOMETRY_ENABLED == ENABLED
+    SCHED_TASK_CLASS(AP_VisualOdom,       &rover.g2.visual_odom,   update,         50,  200),
+#endif
     SCHED_TASK(update_wheel_encoder,   50,    200),
     SCHED_TASK(update_compass,         10,    200),
     SCHED_TASK(update_mission,         50,    200),

APMrover2/Rover.h

@@ -336,9 +336,6 @@ private:
     // Store the time the last GPS message was received.
     uint32_t last_gps_msg_ms{0};
 
-    // last visual odometry update time
-    uint32_t visual_odom_last_update_ms;
-
     // last wheel encoder update times
     float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES];     // distance in radians at time of last update to EKF
     float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES];    // distance in meters at time of last update to EKF (for reporting to GCS)
@@ -493,7 +490,6 @@ private:
     void init_compass_location(void);
     void init_beacon();
     void init_visual_odom();
-    void update_visual_odom();
     void update_wheel_encoder();
     void compass_cal_update(void);
     void compass_save(void);

APMrover2/sensors.cpp

@@ -44,27 +44,6 @@ void Rover::init_visual_odom()
     g2.visual_odom.init();
 }
 
-// update visual odometry sensor
-void Rover::update_visual_odom()
-{
-    // 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();
-        const 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());
-    }
-}
-
 // update wheel encoders
 void Rover::update_wheel_encoder()
 {