diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp
new file mode 100644
index 0000000000000..35ce04ab54014
--- /dev/null
+++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp
@@ -0,0 +1,105 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_VisualOdom.h"
+#include "AP_VisualOdom_Backend.h"
+#include "AP_VisualOdom_MAV.h"
+
+extern const AP_HAL::HAL &hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
+
+ // @Param: _TYPE
+ // @DisplayName: Visual odometry camera connection type
+ // @Description: Visual odometry camera connection type
+ // @Values: 0:None,1:MAV
+ // @User: Advanced
+ AP_GROUPINFO("_TYPE", 0, AP_VisualOdom, _type, 0),
+
+ // @Param: _POS_X
+ // @DisplayName: Visual odometry camera X position offset
+ // @Description: X position of the camera in body frame. Positive X is forward of the origin.
+ // @Units: m
+ // @User: Advanced
+
+ // @Param: _POS_Y
+ // @DisplayName: Visual odometry camera Y position offset
+ // @Description: Y position of the camera in body frame. Positive Y is to the right of the origin.
+ // @Units: m
+ // @User: Advanced
+
+ // @Param: _POS_Z
+ // @DisplayName: Visual odometry camera Z position offset
+ // @Description: Z position of the camera in body frame. Positive Z is down from the origin.
+ // @Units: m
+ // @User: Advanced
+ AP_GROUPINFO("_POS", 1, AP_VisualOdom, _pos_offset, 0.0f),
+
+ // @Param: _ORIENT
+ // @DisplayName: Visual odometery camera orientation
+ // @Description: Visual odometery camera orientation
+ // @Values: 0:Forward, 2:Right, 4:Back, 6:Left, 24:Up, 25:Down
+ // @User: Advanced
+ AP_GROUPINFO("_ORIENT", 2, AP_VisualOdom, _orientation, ROTATION_NONE),
+
+ AP_GROUPEND
+};
+
+AP_VisualOdom::AP_VisualOdom()
+{
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+// detect and initialise any sensors
+void AP_VisualOdom::init()
+{
+ // create backend
+ if (_type == AP_VisualOdom_Type_MAV) {
+ _driver = new AP_VisualOdom_MAV(*this);
+ }
+}
+
+// return true if sensor is enabled
+bool AP_VisualOdom::enabled() const
+{
+ return ((_type != AP_VisualOdom_Type_None) && (_driver != nullptr));
+}
+
+// return true if sensor is basically healthy (we are receiving data)
+bool AP_VisualOdom::healthy() const
+{
+ if (!enabled()) {
+ return false;
+ }
+
+ // healthy if we have received sensor messages within the past 300ms
+ return ((AP_HAL::millis() - _state.last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
+}
+
+// consume VISION_POSITION_DELTA MAVLink message
+void AP_VisualOdom::handle_msg(mavlink_message_t *msg)
+{
+ // exit immediately if not enabled
+ if (!enabled()) {
+ return;
+ }
+
+ // call backend
+ if (_driver != nullptr) {
+ _driver->handle_msg(msg);
+ }
+}
+
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h
new file mode 100644
index 0000000000000..332e8190c8025
--- /dev/null
+++ b/libraries/AP_VisualOdom/AP_VisualOdom.h
@@ -0,0 +1,85 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+class AP_VisualOdom_Backend;
+
+#define AP_VISUALODOM_TIMEOUT_MS 300
+
+class AP_VisualOdom
+{
+public:
+ friend class AP_VisualOdom_Backend;
+
+ AP_VisualOdom();
+
+ // external position backend types (used by _TYPE parameter)
+ enum AP_VisualOdom_Type {
+ AP_VisualOdom_Type_None = 0,
+ AP_VisualOdom_Type_MAV = 1
+ };
+
+ // The VisualOdomState structure is filled in by the backend driver
+ struct VisualOdomState {
+ Vector3f angle_delta; // attitude delta (in radians) of most recent update
+ Vector3f position_delta; // position delta (in meters) of most recent update
+ uint64_t time_delta_usec; // time delta (in usec) between previous and most recent update
+ float confidence; // confidence expressed as a value from 0 (no confidence) to 100 (very confident)
+ uint32_t last_update_ms; // system time (in milliseconds) of last update from sensor
+ };
+
+ // detect and initialise any sensors
+ void init();
+
+ // return true if sensor is enabled
+ bool enabled() const;
+
+ // return true if sensor is basically healthy (we are receiving data)
+ bool healthy() const;
+
+ // state accessors
+ const Vector3f &get_angle_delta() const { return _state.angle_delta; }
+ const Vector3f &get_position_delta() const { return _state.position_delta; }
+ uint64_t get_time_delta_usec() const { return _state.time_delta_usec; }
+ float get_confidence() const { return _state.confidence; }
+ uint32_t get_last_update_ms() const { return _state.last_update_ms; }
+
+ // return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
+ const Vector3f &get_pos_offset(void) const { return _pos_offset; }
+
+ // consume VISUAL_POSITION_DELTA data from MAVLink messages
+ void handle_msg(mavlink_message_t *msg);
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+ // parameters
+ AP_Int8 _type;
+ AP_Vector3f _pos_offset; // position offset of the camera in the body frame
+ AP_Int8 _orientation; // camera orientation on vehicle frame
+
+ // reference to backends
+ AP_VisualOdom_Backend *_driver;
+
+ // state of backend
+ VisualOdomState _state;
+};
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
new file mode 100644
index 0000000000000..93290bb2963bd
--- /dev/null
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
@@ -0,0 +1,41 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_VisualOdom_Backend.h"
+
+/*
+ base class constructor.
+ This incorporates initialisation as well.
+*/
+AP_VisualOdom_Backend::AP_VisualOdom_Backend(AP_VisualOdom &frontend) :
+ _frontend(frontend)
+{
+}
+
+// set deltas (used by backend to update state)
+void AP_VisualOdom_Backend::set_deltas(const Vector3f &angle_delta, const Vector3f& position_delta, uint64_t time_delta_usec, float confidence)
+{
+ // rotate and store angle_delta
+ _frontend._state.angle_delta = angle_delta;
+ _frontend._state.angle_delta.rotate((enum Rotation)_frontend._orientation.get());
+
+ // rotate and store position_delta
+ _frontend._state.position_delta = position_delta;
+ _frontend._state.position_delta.rotate((enum Rotation)_frontend._orientation.get());
+
+ _frontend._state.time_delta_usec = time_delta_usec;
+ _frontend._state.confidence = confidence;
+ _frontend._state.last_update_ms = AP_HAL::millis();
+}
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
new file mode 100644
index 0000000000000..895e8c2f21657
--- /dev/null
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
@@ -0,0 +1,39 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include
+#include
+#include "AP_VisualOdom.h"
+
+class AP_VisualOdom_Backend
+{
+public:
+ // constructor. This incorporates initialisation as well.
+ AP_VisualOdom_Backend(AP_VisualOdom &frontend);
+
+ // consume VISION_POSITION_DELTA MAVLink message
+ virtual void handle_msg(mavlink_message_t *msg) {};
+
+protected:
+
+ // set deltas (used by backend to update state)
+ void set_deltas(const Vector3f &angle_delta, const Vector3f& position_delta, uint64_t time_delta_usec, float confidence);
+
+private:
+
+ // references
+ AP_VisualOdom &_frontend;
+};
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
new file mode 100644
index 0000000000000..32293b11616f3
--- /dev/null
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
@@ -0,0 +1,38 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include
+#include "AP_VisualOdom_MAV.h"
+#include
+
+extern const AP_HAL::HAL& hal;
+
+// constructor
+AP_VisualOdom_MAV::AP_VisualOdom_MAV(AP_VisualOdom &frontend) :
+ AP_VisualOdom_Backend(frontend)
+{
+}
+
+// consume VISIOIN_POSITION_DELTA MAVLink message
+void AP_VisualOdom_MAV::handle_msg(mavlink_message_t *msg)
+{
+ // decode message
+ mavlink_vision_position_delta_t packet;
+ mavlink_msg_vision_position_delta_decode(msg, &packet);
+
+ const Vector3f angle_delta(packet.angle_delta[0], packet.angle_delta[1], packet.angle_delta[2]);
+ const Vector3f position_delta(packet.position_delta[0], packet.position_delta[1], packet.position_delta[2]);
+ set_deltas(angle_delta, position_delta, packet.time_delta_usec, packet.confidence);
+}
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
new file mode 100644
index 0000000000000..1660a967d7e15
--- /dev/null
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "AP_VisualOdom_Backend.h"
+
+class AP_VisualOdom_MAV : public AP_VisualOdom_Backend
+{
+
+public:
+ // constructor
+ AP_VisualOdom_MAV(AP_VisualOdom &frontend);
+
+ // consume VISION_POSITION_DELTA MAVLink message
+ void handle_msg(mavlink_message_t *msg) override;
+};