From 595d37ec70e2feca56a36a77ed213e754577597a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Mar 2017 20:17:45 +0900 Subject: [PATCH] AP_VisualOdom: class accepts deltas from visual odom camera --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 105 ++++++++++++++++++ libraries/AP_VisualOdom/AP_VisualOdom.h | 85 ++++++++++++++ .../AP_VisualOdom/AP_VisualOdom_Backend.cpp | 41 +++++++ .../AP_VisualOdom/AP_VisualOdom_Backend.h | 39 +++++++ libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 38 +++++++ libraries/AP_VisualOdom/AP_VisualOdom_MAV.h | 14 +++ 6 files changed, 322 insertions(+) create mode 100644 libraries/AP_VisualOdom/AP_VisualOdom.cpp create mode 100644 libraries/AP_VisualOdom/AP_VisualOdom.h create mode 100644 libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp create mode 100644 libraries/AP_VisualOdom/AP_VisualOdom_Backend.h create mode 100644 libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp create mode 100644 libraries/AP_VisualOdom/AP_VisualOdom_MAV.h diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp new file mode 100644 index 0000000000..35ce04ab54 --- /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 0000000000..332e8190c8 --- /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 0000000000..93290bb296 --- /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 0000000000..895e8c2f21 --- /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 0000000000..32293b1161 --- /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 0000000000..1660a967d7 --- /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; +};