You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
38 lines
992 B
38 lines
992 B
#include "AP_DAL_VisualOdom.h" |
|
|
|
#include <AP_VisualOdom/AP_VisualOdom.h> |
|
|
|
#if HAL_VISUALODOM_ENABLED |
|
|
|
#include <AP_Logger/AP_Logger.h> |
|
#include "AP_DAL.h" |
|
|
|
/* |
|
update position offsets to align to AHRS position |
|
should only be called when this library is not being used as the position source |
|
This function does not change EKF state, so does not need to be logged |
|
*/ |
|
void AP_DAL_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z) |
|
{ |
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) |
|
auto *vo = AP::visualodom(); |
|
vo->align_position_to_ahrs(align_xy, align_z); |
|
#endif |
|
} |
|
|
|
void AP_DAL_VisualOdom::start_frame() |
|
{ |
|
const auto *vo = AP::visualodom(); |
|
|
|
const log_RVOH old = RVOH; |
|
if (vo != nullptr) { |
|
RVOH.pos_offset = vo->get_pos_offset(); |
|
RVOH.delay_ms = vo->get_delay_ms(); |
|
RVOH.healthy = vo->healthy(); |
|
RVOH.enabled = vo->enabled(); |
|
} |
|
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(RVOH, RVOH, old); |
|
} |
|
|
|
#endif // HAL_VISUALODOM_ENABLED
|
|
|