2020-11-05 19:22:38 -04:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <AP_Logger/LogStructure.h>
|
|
|
|
|
|
|
|
#include <AP_VisualOdom/AP_VisualOdom.h>
|
|
|
|
|
|
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
|
|
|
2020-11-06 06:21:46 -04:00
|
|
|
#if HAL_VISUALODOM_ENABLED
|
|
|
|
|
2020-11-05 19:22:38 -04:00
|
|
|
class AP_DAL_VisualOdom {
|
|
|
|
public:
|
|
|
|
|
|
|
|
// return VisualOdom health
|
|
|
|
bool healthy() const {
|
|
|
|
return RVOH.healthy;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool enabled() const {
|
|
|
|
return RVOH.enabled;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool get_delay_ms() const {
|
|
|
|
return RVOH.delay_ms;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
|
|
|
|
const Vector3f &get_pos_offset() const {
|
|
|
|
return RVOH.pos_offset;
|
|
|
|
}
|
|
|
|
|
2020-11-08 21:28:58 -04:00
|
|
|
// update position offsets to align to AHRS position
|
|
|
|
// should only be called when this library is not being used as the position source
|
|
|
|
void align_position_to_ahrs(bool align_xy, bool align_z);
|
|
|
|
|
2020-11-05 19:22:38 -04:00
|
|
|
void start_frame();
|
|
|
|
|
|
|
|
void handle_message(const log_RVOH &msg) {
|
|
|
|
RVOH = msg;
|
2020-11-08 21:28:58 -04:00
|
|
|
}
|
2020-11-05 19:22:38 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
|
|
|
|
struct log_RVOH RVOH;
|
|
|
|
};
|
2020-11-06 06:21:46 -04:00
|
|
|
|
|
|
|
#endif // HAL_VISUALODOM_ENABLED
|