diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp
index ba4f96454c..7d41ec330d 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp
@@ -90,36 +90,7 @@ void AP_VisualOdom::init()
// return true if sensor is enabled
bool AP_VisualOdom::enabled() const
{
- return ((_type != AP_VisualOdom_Type_None) && (_driver != nullptr));
-}
-
-// update visual odometry sensor
-void AP_VisualOdom::update()
-{
- if (!enabled()) {
- return;
- }
-
- // check for updates
- if (_state.last_processed_sensor_update_ms == _state.last_sensor_update_ms) {
- // This reading has already been processed
- return;
- }
- _state.last_processed_sensor_update_ms = _state.last_sensor_update_ms;
-
- const float time_delta_sec = get_time_delta_usec() / 1000000.0f;
-
- AP::ahrs_navekf().writeBodyFrameOdom(get_confidence(),
- get_position_delta(),
- get_angle_delta(),
- time_delta_sec,
- get_last_update_ms(),
- get_pos_offset());
- // log sensor data
- AP::logger().Write_VisualOdom(time_delta_sec,
- get_angle_delta(),
- get_position_delta(),
- get_confidence());
+ return ((_type != AP_VisualOdom_Type_None));
}
// return true if sensor is basically healthy (we are receiving data)
@@ -129,8 +100,10 @@ bool AP_VisualOdom::healthy() const
return false;
}
- // healthy if we have received sensor messages within the past 300ms
- return ((AP_HAL::millis() - _state.last_sensor_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
+ if (_driver == nullptr) {
+ return false;
+ }
+ return _driver->healthy();
}
// consume VISION_POSITION_DELTA MAVLink message
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h
index 63ea267fa3..01801b1c00 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom.h
@@ -27,6 +27,7 @@ class AP_VisualOdom
{
public:
friend class AP_VisualOdom_Backend;
+ friend class AP_VisualOdom_MAV;
AP_VisualOdom();
@@ -41,23 +42,9 @@ public:
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_sensor_update_ms; // system time (in milliseconds) of last update from sensor
- uint32_t last_processed_sensor_update_ms; // timestamp of last sensor update that was processed
- };
-
// detect and initialise any sensors
void init();
- // should be called really, really often. The faster you call
- // this the lower the latency of the data fed to the estimator.
- void update();
-
// return true if sensor is enabled
bool enabled() const;
@@ -87,12 +74,8 @@ private:
static AP_VisualOdom *_singleton;
- // 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_sensor_update_ms; }
+ // get user defined orientation
+ enum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }
// parameters
AP_Int8 _type;
@@ -101,9 +84,6 @@ private:
// reference to backends
AP_VisualOdom_Backend *_driver;
-
- // state of backend
- VisualOdomState _state;
};
namespace AP {
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
index efeaad3b89..9094b0f844 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
@@ -28,20 +28,11 @@ AP_VisualOdom_Backend::AP_VisualOdom_Backend(AP_VisualOdom &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)
+// return true if sensor is basically healthy (we are receiving data)
+bool AP_VisualOdom_Backend::healthy() const
{
- // 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_sensor_update_ms = AP_HAL::millis();
+ // healthy if we have received sensor messages within the past 300ms
+ return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
}
// general purpose method to send position estimate data to EKF
@@ -88,6 +79,9 @@ void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time
// store corrected attitude for use in pre-arm checks
_attitude_last = att;
_have_attitude = true;
+
+ // record time for health monitoring
+ _last_update_ms = AP_HAL::millis();
}
// apply rotation and correction to position
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
index 3c9e9eecb2..1e031d46bd 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
@@ -25,6 +25,9 @@ public:
// constructor. This incorporates initialisation as well.
AP_VisualOdom_Backend(AP_VisualOdom &frontend);
+ // return true if sensor is basically healthy (we are receiving data)
+ bool healthy() const;
+
// consume VISION_POSITION_DELTA MAVLink message
virtual void handle_msg(const mavlink_message_t &msg) {};
@@ -41,9 +44,6 @@ public:
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);
-
// apply rotation and correction to position
void rotate_and_correct_position(Vector3f &position) const;
@@ -53,9 +53,8 @@ protected:
// use sensor provided position and attitude to calculate rotation to align sensor with AHRS/EKF attitude
bool align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude);
-private:
-
AP_VisualOdom &_frontend; // reference to frontend
+
float _yaw_trim; // yaw angle trim (in radians) to align camera's yaw to ahrs/EKF's
Quaternion _yaw_rotation; // earth-frame yaw rotation to align heading of sensor with vehicle. use when _yaw_trim is non-zero
Quaternion _att_rotation; // body-frame rotation corresponding to ORIENT parameter. use when get_orientation != NONE
@@ -67,4 +66,5 @@ private:
bool _have_attitude; // true if we have received an attitude from the camera (used for arming checks)
bool _error_orientation; // true if the orientation is not supported
Quaternion _attitude_last; // last attitude received from camera (used for arming checks)
+ uint32_t _last_update_ms; // system time of last update from sensor (used by health checks)
};
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
index a6bd7ba3c9..255b478ab3 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
@@ -13,9 +13,10 @@
along with this program. If not, see .
*/
-#include
#include "AP_VisualOdom_MAV.h"
-#include
+#include
+#include
+#include
extern const AP_HAL::HAL& hal;
@@ -32,7 +33,27 @@ void AP_VisualOdom_MAV::handle_msg(const mavlink_message_t &msg)
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);
+ // apply rotation to angle and position delta
+ const enum Rotation rot = _frontend.get_orientation();
+ Vector3f angle_delta = Vector3f(packet.angle_delta[0], packet.angle_delta[1], packet.angle_delta[2]);
+ angle_delta.rotate(rot);
+ Vector3f position_delta = Vector3f(packet.position_delta[0], packet.position_delta[1], packet.position_delta[2]);
+ position_delta.rotate(rot);
+
+ const uint32_t now_ms = AP_HAL::millis();
+ _last_update_ms = now_ms;
+
+ // send to EKF
+ AP::ahrs_navekf().writeBodyFrameOdom(packet.confidence,
+ angle_delta,
+ position_delta,
+ packet.time_delta_usec,
+ now_ms,
+ _frontend.get_pos_offset());
+
+ // log sensor data
+ AP::logger().Write_VisualOdom(packet.time_delta_usec / 1000000.0f,
+ angle_delta,
+ position_delta,
+ packet.confidence);
}