Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_VisualOdom: rename align_sensor_to_vehicle to align_yaw_to_ahrs 

also separate request to align from code that actually performs the alignment
This commit is contained in:
Randy Mackay 2023-01-20 16:22:04 +09:00 committed by Andrew Tridgell
parent c3f5a539d2
commit 8336814607
5 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -218,8 +218,8 @@ void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32
}
}
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
void AP_VisualOdom::align_sensor_to_vehicle()
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
void AP_VisualOdom::request_align_yaw_to_ahrs()
{
// exit immediately if not enabled
if (!enabled()) {
@ -228,7 +228,7 @@ void AP_VisualOdom::align_sensor_to_vehicle()
// call backend
if (_driver != nullptr) {
_driver->align_sensor_to_vehicle();
_driver->request_align_yaw_to_ahrs();
}
}

4
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -93,8 +93,8 @@ public:
// velocity in NED meters per second
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter);
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
void align_sensor_to_vehicle();
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
void request_align_yaw_to_ahrs();
// update position offsets to align to AHRS position
// should only be called when this library is not being used as the position source

4
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -36,8 +36,8 @@ public:
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;
// handle request to align camera's attitude with vehicle's AHRS/EKF attitude
virtual void align_sensor_to_vehicle() {}
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
virtual void request_align_yaw_to_ahrs() {}
// handle request to align position with AHRS
virtual void align_position_to_ahrs(bool align_xy, bool align_z) {}

10
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -33,10 +33,10 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
Quaternion att = attitude;
// handle user request to align camera
if (_align_camera) {
if (align_sensor_to_vehicle(pos, attitude)) {
_align_camera = false;
// handle request to align sensor's yaw with vehicle's AHRS/EKF attitude
if (_align_yaw) {
if (align_yaw_to_ahrs(pos, attitude)) {
_align_yaw = false;
}
}
if (_align_posxy || _align_posz) {
@ -128,7 +128,7 @@ void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const
}
// use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude)
bool AP_VisualOdom_IntelT265::align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude)
{
// do not align to ahrs if we are its yaw source
if (AP::ahrs().using_extnav_for_yaw()) {

10
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
View File

@ -17,8 +17,8 @@ public:
// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, 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; }
// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
void request_align_yaw_to_ahrs() override { _align_yaw = true; }
// update position offsets to align to AHRS position
// should only be called when this library is not being used as the position source
@ -38,8 +38,8 @@ protected:
// rotate attitude using _yaw_trim
void rotate_attitude(Quaternion &attitude) const;
// 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);
// use sensor provided position and attitude to calculate rotation to align sensor yaw with AHRS/EKF attitude
bool align_yaw_to_ahrs(const Vector3f &position, const Quaternion &attitude);
// returns true if sensor data should be consumed, false if it should be ignored
// set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise
@ -57,7 +57,7 @@ protected:
Vector3f _pos_correction; // position correction that should be added to position reported from sensor
bool _use_att_rotation; // true if _att_rotation should be applied to sensor's attitude data
bool _use_posvel_rotation; // true if _posvel_rotation should be applied to sensor's position and/or velocity data
bool _align_camera = true; // true if camera should be aligned to AHRS/EKF
bool _align_yaw = true; // true if sensor yaw should be aligned to AHRS/EKF
bool _align_posxy; // true if sensor xy position should be aligned to AHRS
bool _align_posz; // true if sensor z position should be aligned to AHRS
bool _error_orientation; // true if the orientation is not supported