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

AP_DAL: allow for more than 327m range rangefinders

Browse Source
This commit is contained in:
Peter Barker 2024-10-11 14:46:47 +11:00 committed by Andrew Tridgell
parent 7ff52a71fb
commit c681bb4934
3 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_DAL/AP_DAL_RangeFinder.cpp
View File

@ -36,7 +36,7 @@ failed:
#endif
}
int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
float AP_DAL_RangeFinder::ground_clearance_orient(enum Rotation orientation) const
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
const auto *rangefinder = AP::rangefinder();
@ -44,25 +44,25 @@ int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation
if (orientation != ROTATION_PITCH_270) {
// the EKF only asks for this from a specific orientation. Thankfully.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return rangefinder->ground_clearance_cm_orient(orientation);
return rangefinder->ground_clearance_orient(orientation);
}
#endif
return _RRNH.ground_clearance_cm;
return _RRNH.ground_clearance;
}
int16_t AP_DAL_RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
float AP_DAL_RangeFinder::max_distance_orient(enum Rotation orientation) const
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
if (orientation != ROTATION_PITCH_270) {
const auto *rangefinder = AP::rangefinder();
// the EKF only asks for this from a specific orientation. Thankfully.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return rangefinder->max_distance_cm_orient(orientation);
return rangefinder->max_distance_orient(orientation);
}
#endif
return _RRNH.max_distance_cm;
return _RRNH.max_distance;
}
void AP_DAL_RangeFinder::start_frame()
@ -75,8 +75,8 @@ void AP_DAL_RangeFinder::start_frame()
const log_RRNH old = _RRNH;
// EKF only asks for this *down*.
_RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);
_RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270);
_RRNH.ground_clearance = rangefinder->ground_clearance_orient(ROTATION_PITCH_270);
_RRNH.max_distance = rangefinder->max_distance_orient(ROTATION_PITCH_270);
WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old);
@ -102,7 +102,7 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
_RRNI.orientation = backend->orientation();
_RRNI.status = (uint8_t)backend->status();
_RRNI.pos_offset = backend->get_pos_offset();
_RRNI.distance_cm = backend->distance_cm();
_RRNI.distance = backend->distance();
WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
}

6
libraries/AP_DAL/AP_DAL_RangeFinder.h
View File

@ -20,8 +20,8 @@ public:
Good
};
int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
int16_t max_distance_cm_orient(enum Rotation orientation) const;
float ground_clearance_orient(enum Rotation orientation) const;
float max_distance_orient(enum Rotation orientation) const;
// return true if we have a range finder with the specified orientation
bool has_orientation(enum Rotation orientation) const;
@ -59,7 +59,7 @@ public:
return (AP_DAL_RangeFinder::Status)_RRNI.status;
}
uint16_t distance_cm() const { return _RRNI.distance_cm; }
float distance() const { return _RRNI.distance; }
const Vector3f &get_pos_offset() const { return _RRNI.pos_offset; }

10
libraries/AP_DAL/LogStructure.h
View File

@ -172,8 +172,8 @@ struct log_RBRI {
// @Description: Replay Data Rangefinder Header
struct log_RRNH {
// this is rotation-pitch-270!
int16_t ground_clearance_cm;
int16_t max_distance_cm;
float ground_clearance;
float max_distance;
uint8_t num_sensors;
uint8_t _end;
};
@ -182,7 +182,7 @@ struct log_RRNH {
// @Description: Replay Data Rangefinder Instance
struct log_RRNI {
Vector3f pos_offset;
uint16_t distance_cm;
float distance;
uint8_t orientation;
uint8_t status;
uint8_t instance;
@ -420,9 +420,9 @@ struct log_RBOH {
{ LOG_RBRI_MSG, RLOG_SIZE(RBRI), \
"RBRI", "IfBB", "LastUpdate,Alt,H,I", "---#", "----" }, \
{ LOG_RRNH_MSG, RLOG_SIZE(RRNH), \
"RRNH", "hhB", "GCl,MaxD,NumSensors", "???", "???" }, \
"RRNH", "ffB", "GCl,MaxD,NumSensors", "mm-", "00-" }, \
{ LOG_RRNI_MSG, RLOG_SIZE(RRNI), \
"RRNI", "fffHBBB", "PX,PY,PZ,Dist,Orient,Status,I", "------#", "-------" }, \
"RRNI", "ffffBBB", "PX,PY,PZ,Dist,Orient,Status,I", "---m--#", "---0---" }, \
{ LOG_RGPH_MSG, RLOG_SIZE(RGPH), \
"RGPH", "BB", "NumInst,Primary", "--", "--" }, \
{ LOG_RGPI_MSG, RLOG_SIZE(RGPI), \