aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_VisualOdom: added VOXL backend type

Browse Source 
this will make it easier to have custom behaviour for VOXL
This commit is contained in:
Andrew Tridgell 2021-12-18 17:08:13 +11:00
parent 8654ea886d
commit 97fee2d1cc
4 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_VisualOdom/AP_VisualOdom.cpp
View File

@ -31,7 +31,7 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
// @Param: _TYPE // @Param: _TYPE
// @DisplayName: Visual odometry camera connection type // @DisplayName: Visual odometry camera connection type
// @Description: Visual odometry camera connection type // @Description: Visual odometry camera connection type
// @Values: 0:None,1:MAVLink,2:IntelT265 // @Values: 0:None,1:MAVLink,2:IntelT265,3:VOXL(ModalAI)
// @User: Advanced // @User: Advanced
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO_FLAGS("_TYPE", 0, AP_VisualOdom, _type, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("_TYPE", 0, AP_VisualOdom, _type, 0, AP_PARAM_FLAG_ENABLE),
@ -124,14 +124,15 @@ AP_VisualOdom::AP_VisualOdom()
void AP_VisualOdom::init() void AP_VisualOdom::init()
{ {
// create backend // create backend
switch (_type) { switch (VisualOdom_Type(_type.get())) {
case AP_VisualOdom_Type_None: case VisualOdom_Type::None:
// do nothing // do nothing
break; break;
case AP_VisualOdom_Type_MAV: case VisualOdom_Type::MAV:
_driver = new AP_VisualOdom_MAV(*this); _driver = new AP_VisualOdom_MAV(*this);
break; break;
case AP_VisualOdom_Type_IntelT265: case VisualOdom_Type::IntelT265:
case VisualOdom_Type::VOXL:
_driver = new AP_VisualOdom_IntelT265(*this); _driver = new AP_VisualOdom_IntelT265(*this);
break; break;
} }
@ -140,7 +141,7 @@ void AP_VisualOdom::init()
// return true if sensor is enabled // return true if sensor is enabled
bool AP_VisualOdom::enabled() const bool AP_VisualOdom::enabled() const
{ {
return ((_type != AP_VisualOdom_Type_None)); return ((_type != VisualOdom_Type::None));
} }
// return true if sensor is basically healthy (we are receiving data) // return true if sensor is basically healthy (we are receiving data)

15
libraries/AP_VisualOdom/AP_VisualOdom.h
View File

@ -44,10 +44,11 @@ public:
} }
// external position backend types (used by _TYPE parameter) // external position backend types (used by _TYPE parameter)
enum AP_VisualOdom_Type { enum class VisualOdom_Type {
AP_VisualOdom_Type_None = 0, None = 0,
AP_VisualOdom_Type_MAV = 1, MAV = 1,
AP_VisualOdom_Type_IntelT265 = 2 IntelT265 = 2,
VOXL = 3,
}; };
// detect and initialise any sensors // detect and initialise any sensors
@ -106,12 +107,16 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
VisualOdom_Type get_type(void) const {
return _type;
}
private: private:
static AP_VisualOdom *_singleton; static AP_VisualOdom *_singleton;
// parameters // parameters
AP_Int8 _type; // sensor type AP_Enum<VisualOdom_Type> _type; // sensor type
AP_Vector3f _pos_offset; // position offset of the camera in the body frame AP_Vector3f _pos_offset; // position offset of the camera in the body frame
AP_Int8 _orientation; // camera orientation on vehicle frame AP_Int8 _orientation; // camera orientation on vehicle frame
AP_Float _pos_scale; // position scale factor applied to sensor values AP_Float _pos_scale; // position scale factor applied to sensor values

4
libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
View File

@ -51,6 +51,10 @@ protected:
// updates the reset timestamp to the current system time if the reset_counter has 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); uint32_t get_reset_timestamp_ms(uint8_t reset_counter);
AP_VisualOdom::VisualOdom_Type get_type(void) const {
return _frontend.get_type();
}
// Logging Functions // Logging Functions
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence); void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored); void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored);

5
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -261,6 +261,11 @@ bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_m
// only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored
bool AP_VisualOdom_IntelT265::should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter) bool AP_VisualOdom_IntelT265::should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter)
{ {
if (get_type() == AP_VisualOdom::VisualOdom_Type::VOXL) {
// we don't discard data after a reset for VOXL
return true;
}
uint32_t now_ms = AP_HAL::millis(); uint32_t now_ms = AP_HAL::millis();
// set ignore start time if reset counter has changed // set ignore start time if reset counter has changed