AP_VisualOdom: class accepts deltas from visual odom camera

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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);
+    }
+}
+

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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+
+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;
+};

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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();
+}

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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#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;
+};

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_VisualOdom_MAV.h"
+#include <AP_SerialManager/AP_SerialManager.h>
+
+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);
+}

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;
+};