Rover: add support for visual odometry

APMrover2/APMrover2.cpp

@@ -52,6 +52,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(update_GPS_10Hz,        10,   2500),
     SCHED_TASK(update_alt,             10,   3400),
     SCHED_TASK(update_beacon,          50,     50),
+    SCHED_TASK(update_visual_odom,     50,     50),
     SCHED_TASK(navigate,               10,   1600),
     SCHED_TASK(update_compass,         10,   2000),
     SCHED_TASK(update_commands,        10,   1000),

APMrover2/GCS_Mavlink.cpp

@@ -1572,6 +1572,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
         AP_Notify::handle_play_tune(msg);
         break;
 
+    case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
+        rover.g2.visual_odom.handle_msg(msg);
+        break;
+
     default:
         handle_common_message(msg);
         break;

APMrover2/Parameters.cpp

@@ -548,6 +548,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp
     AP_SUBGROUPINFO(beacon, "BCN", 6, ParametersG2, AP_Beacon),
 
+    // @Group: VISO
+    // @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp
+    AP_SUBGROUPINFO(visual_odom, "VISO", 7, ParametersG2, AP_VisualOdom),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -319,6 +319,9 @@ public:
     AP_AdvancedFailsafe_Rover afs;
 #endif
     AP_Beacon beacon;
+
+    // Visual Odometry camera
+    AP_VisualOdom visual_odom;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -73,6 +73,7 @@
 #include <AP_Button/AP_Button.h>
 #include <AP_Stats/AP_Stats.h>                      // statistics library
 #include <AP_Beacon/AP_Beacon.h>
+#include <AP_VisualOdom/AP_VisualOdom.h>
 
 // Configuration
 #include "config.h"
@@ -402,6 +403,9 @@ 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;
+
 private:
     // private member functions
     void ahrs_update();
@@ -507,6 +511,8 @@ private:
     void init_sonar(void);
     void init_beacon();
     void update_beacon();
+    void init_visual_odom();
+    void update_visual_odom();
     void read_battery(void);
     void read_receiver_rssi(void);
     void read_sonars(void);

APMrover2/sensors.cpp

@@ -28,6 +28,33 @@ void Rover::update_beacon()
     g2.beacon.update();
 }
 
+// init visual odometry sensor
+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
+        DataFlash.Log_Write_VisualOdom(time_delta_sec,
+                                       g2.visual_odom.get_angle_delta(),
+                                       g2.visual_odom.get_position_delta(),
+                                       g2.visual_odom.get_confidence());
+    }
+}
+
 // read_battery - reads battery voltage and current and invokes failsafe
 // should be called at 10hz
 void Rover::read_battery(void)
@@ -154,7 +181,9 @@ void Rover::update_sensor_status_flags(void)
     if (gps.status() > AP_GPS::NO_GPS) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
     }
-
+    if (g2.visual_odom.enabled()) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
+    }
     if (rover.DataFlash.logging_present()) {  // primary logging only (usually File)
         control_sensors_present |= MAV_SYS_STATUS_LOGGING;
     }
@@ -209,6 +238,9 @@ void Rover::update_sensor_status_flags(void)
     if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
+    if (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) {
+        control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
+    }
     if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
     }

APMrover2/system.cpp

@@ -161,6 +161,9 @@ void Rover::init_ardupilot()
     // init beacons used for non-gps position estimation
     init_beacon();
 
+    // init visual odometry
+    init_visual_odom();
+
     // and baro for EKF
     init_barometer(true);