Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: handle_msg directly updates EKF

Browse Source
This commit is contained in:
Randy Mackay 2020-03-30 14:30:42 +09:00
parent 0eb1ef1f08
commit c36dfc448d
5 changed files with 46 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -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

26
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -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 {

20
libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
View File

@ -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

10
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -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)
};

31
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -13,9 +13,10 @@
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>
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
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);
}