Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 04:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: support position resets

Browse Source
This commit is contained in:
Randy Mackay 2020-04-10 13:36:26 +09:00 committed by Andrew Tridgell
parent 83e48b575e
commit 6622927a23
8 changed files with 34 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -140,7 +140,7 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms
// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw)
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter)
{
// exit immediately if not enabled
if (!enabled()) {
@ -152,12 +152,12 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// convert attitude to quaternion and call backend
Quaternion attitude;
attitude.from_euler(roll, pitch, yaw);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
}
}
// general purpose method to consume position estimate data and send to EKF
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
{
// exit immediately if not enabled
if (!enabled()) {
@ -166,7 +166,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
// call backend
if (_driver != nullptr) {
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
_driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
}
}

4
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -73,8 +73,8 @@ public:
// general purpose methods to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter);
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter);
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
void align_sensor_to_vehicle();

12
libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp
View File

@ -38,4 +38,16 @@ bool AP_VisualOdom_Backend::healthy() const
return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
}
// returns the system time of the last reset if reset_counter has not changed
// updates the reset timestamp to the current system time if the reset_counter has changed
uint32_t AP_VisualOdom_Backend::get_reset_timestamp_ms(uint8_t reset_counter)
{
// update reset counter and timestamp if reset_counter has changed
if (reset_counter != _last_reset_counter) {
_last_reset_counter = reset_counter;
_reset_timestamp_ms = AP_HAL::millis();
}
return _reset_timestamp_ms;
}
#endif

9
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -31,7 +31,7 @@ public:
virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) = 0;
// consume vision position estimate data and send to EKF. distances in meters
virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) = 0;
virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) = 0;
// handle request to align camera's attitude with vehicle's AHRS/EKF attitude
virtual void align_sensor_to_vehicle() {}
@ -41,9 +41,16 @@ public:
protected:
// returns the system time of the last reset if reset_counter has not changed
// updates the reset timestamp to the current system time if the reset_counter has changed
uint32_t get_reset_timestamp_ms(uint8_t reset_counter);
AP_VisualOdom &_frontend; // reference to frontend
uint32_t _last_update_ms; // system time of last update from sensor (used by health checks)
// reset counter handling
uint8_t _last_reset_counter; // last sensor reset counter received
uint32_t _reset_timestamp_ms; // time reset counter was received
};
#endif

7
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -24,7 +24,7 @@
extern const AP_HAL::HAL& hal;
// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
@ -44,8 +44,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
// send attitude and position to EKF
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
const uint32_t reset_timestamp_ms = 0; // no data available
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, reset_timestamp_ms);
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
// calculate euler orientation for logging
float roll;
@ -54,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
att.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)));
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter);
// store corrected attitude for use in pre-arm checks
_attitude_last = att;

2
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
View File

@ -15,7 +15,7 @@ public:
void handle_vision_position_delta_msg(const mavlink_message_t &msg) override {};
// consume vision position estimate data and send to EKF. distances in meters
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) override;
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override;
// handle request to align camera's attitude with vehicle's AHRS/EKF attitude
void align_sensor_to_vehicle() override { _align_camera = true; }

7
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -63,7 +63,7 @@ void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t
}
// consume vision position estimate data and send to EKF. distances in meters
void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
{
const float scale_factor = _frontend.get_pos_scale();
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
@ -71,8 +71,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
// send attitude and position to EKF
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
const uint32_t reset_timestamp_ms = 0; // no data available
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, reset_timestamp_ms);
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
// calculate euler orientation for logging
float roll;
@ -81,7 +80,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
attitude.to_euler(roll, pitch, yaw);
// log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw));
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw), reset_counter);
// record time for health monitoring
_last_update_ms = AP_HAL::millis();

2
libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
View File

@ -15,7 +15,7 @@ public:
void handle_vision_position_delta_msg(const mavlink_message_t &msg) override;
// consume vision position estimate data and send to EKF. distances in meters
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) override;
void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override;
};
#endif