Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_RangeFinder: add snr check for USD1 serial rangefinder #29099

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

AP_RangeFinder: add snr check for USD1 serial rangefinder #29099

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6f34a4c
AP_RangeFinder: add snr check for USD1 serial rangefinder
magate Jan 17, 2025
839b1c0
AP_Rangefinder: parameter fixes for USD-D1 serial
rishabsingh3003 Jan 17, 2025
1667a75
add logging for snr
magate Jan 17, 2025
081979c
Fix log structure
magate Jan 17, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion libraries/AP_Logger/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -460,6 +460,7 @@ struct PACKED log_RFND {
uint8_t status;
uint8_t orient;
int8_t quality;
uint8_t snr;
};

/*
Expand Down Expand Up @@ -1181,7 +1182,7 @@ LOG_STRUCTURE_FROM_MOUNT \
{ LOG_MODE_MSG, sizeof(log_Mode), \
"MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
{ LOG_RFND_MSG, sizeof(log_RFND), \
"RFND", "QBCBBb", "TimeUS,Instance,Dist,Stat,Orient,Quality", "s#m--%", "F-B---", true }, \
"RFND", "QBCBBbB", "TimeUS,Instance,Dist,Stat,Orient,Quality,SNR", "s#m--%-", "F-B----", true }, \
{ LOG_MAV_STATS, sizeof(log_MAV_Stats), \
"DMS", "QIIIIBBBBBBBBB", "TimeUS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "F-------------" }, \
LOG_STRUCTURE_FROM_BEACON \
Expand Down
21 changes: 11 additions & 10 deletions libraries/AP_RangeFinder/AP_RangeFinder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params),

// @Group: 1_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]),

#if RANGEFINDER_MAX_INSTANCES > 1
Expand All @@ -89,7 +89,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params),

// @Group: 2_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
#endif

Expand All @@ -99,7 +99,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params),

// @Group: 3_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
#endif

Expand All @@ -109,7 +109,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params),

// @Group: 4_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]),
#endif

Expand All @@ -119,7 +119,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params),

// @Group: 5_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]),
#endif

Expand All @@ -129,7 +129,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params),

// @Group: 6_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]),
#endif

Expand All @@ -139,7 +139,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params),

// @Group: 7_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]),
#endif

Expand All @@ -149,7 +149,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params),

// @Group: 8_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]),
#endif

Expand All @@ -159,7 +159,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params),

// @Group: 9_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]),
#endif

Expand All @@ -169,7 +169,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params),

// @Group: A_
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp
// @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_USD1_Serial.cpp
AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]),
#endif

Expand Down Expand Up @@ -825,6 +825,7 @@ void RangeFinder::Log_RFND() const
status : (uint8_t)s->status(),
orient : s->orientation(),
quality : s->signal_quality_pct(),
snr : s->signal_to_noise_ratio(),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_RangeFinder/AP_RangeFinder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,6 +213,7 @@ class RangeFinder
enum RangeFinder::Status status; // sensor status
uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10)
uint32_t last_reading_ms; // system time of last successful update from sensor
uint8_t snr; // signal to noise ratio, if applicable, otherwise 0

const struct AP_Param::GroupInfo *var_info;
};
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ class AP_RangeFinder_Backend
uint16_t distance_cm() const { return state.distance_m*100.0f; }
int8_t signal_quality_pct() const WARN_IF_UNUSED { return state.signal_quality_pct; }
uint16_t voltage_mv() const { return state.voltage_mv; }
uint8_t signal_to_noise_ratio() const { return state.snr; }
virtual int16_t max_distance_cm() const { return params.max_distance_cm; }
virtual int16_t min_distance_cm() const { return params.min_distance_cm; }
int16_t ground_clearance_cm() const { return params.ground_clearance_cm; }
Expand Down
32 changes: 31 additions & 1 deletion libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,29 @@
#define USD1_HDR 254 // Header Byte from USD1_Serial (0xFE)
#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)

const AP_Param::GroupInfo AP_RangeFinder_USD1_Serial::var_info[] = {
// @Param: SNR_MIN
// @DisplayName: Minimum signal strength
// @Description: Minimum signal strength (SNR) to accept distance
// @Range: 0 65535
// @User: Advanced
AP_GROUPINFO("SNR_MIN", 12, AP_RangeFinder_USD1_Serial, snr_min, 0),

AP_GROUPEND
};

/*
constructor
*/
AP_RangeFinder_USD1_Serial::AP_RangeFinder_USD1_Serial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend_Serial(_state, _params)
{
AP_Param::setup_object_defaults(this, var_info);

state.var_info = var_info;
}

/*
detect USD1_Serial Firmware Version
*/
Expand Down Expand Up @@ -149,7 +172,14 @@ bool AP_RangeFinder_USD1_Serial::get_reading(float &reading_m)
} else {
// evaluate checksum
if (((_linebuf[1] + _linebuf[2] + _linebuf[3] + _linebuf[4]) & 0xFF) == _linebuf[5]) {
// if checksum passed, parse data for Firmware Version #1
// if checksum passed, check snr
const uint8_t snr = _linebuf[4];
state.snr = snr;
if (snr_min !=0 && snr < uint16_t(snr_min.get())) {
// too low signal strength
return true;
}
// checksum and signal strength passed, parse data for Firmware Version #1
sum += _linebuf[3]*256 + _linebuf[2];
count++;
}
Expand Down
8 changes: 6 additions & 2 deletions libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,17 @@ class AP_RangeFinder_USD1_Serial : public AP_RangeFinder_Backend_Serial

public:

AP_RangeFinder_USD1_Serial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params);

static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return NEW_NOTHROW AP_RangeFinder_USD1_Serial(_state, _params);
}

static const struct AP_Param::GroupInfo var_info[];

protected:

MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
Expand All @@ -34,8 +39,6 @@ class AP_RangeFinder_USD1_Serial : public AP_RangeFinder_Backend_Serial

private:

using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;

// detect USD1_Serial Firmware Version
bool detect_version(void);

Expand All @@ -47,6 +50,7 @@ class AP_RangeFinder_USD1_Serial : public AP_RangeFinder_Backend_Serial
bool _version_known;
uint8_t _header;
uint8_t _version;
AP_Int32 snr_min;
};

#endif // AP_RANGEFINDER_USD1_SERIAL_ENABLED
Loading