Skip to content

Commit

Permalink
AP_RangeFinder: reworked the Ainstein driver
Browse files Browse the repository at this point in the history
  • Loading branch information
@magicrub
magicrub committed Jan 14, 2024
1 parent f535837 commit 3d5e4ff
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 129 deletions.
160 changes: 59 additions & 101 deletions libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,132 +21,90 @@

#include "AP_RangeFinder_Ainstein_LR_D1.h"

#include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <GCS_MAVLink/GCS.h>

static const uint8_t PACKET_HEADER_MSB = 0xEB;
static const uint8_t PACKET_HEADER_LSB = 0x90;

// make sure we know what size the packet object is:
// assert_storage_size<AP_RangeFinder_Ainstein_LR_D1::LRD1Union::LRD1Packet, 32> _assert_storage_lrd1_packet;

bool AP_RangeFinder_Ainstein_LR_D1::move_signature_in_buffer(uint8_t start)
{
for (uint8_t i=start; i<buffer_used; i++) {
if (u.buffer[i] == PACKET_HEADER_MSB) {
memmove(&u.buffer[0], &u.buffer[i], buffer_used-i);
buffer_used -= i;
return true;
}
}
// header byte not in buffer
buffer_used = 0;
return false;
}

uint8_t AP_RangeFinder_Ainstein_LR_D1::LRD1Union::calculate_checksum() const
{
uint32_t sum = 0;
for (uint8_t i=3; i<31; i++) {
sum += buffer[i];
}
return sum & 0xff;
}

// get_reading - read a value from the sensor
bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
{
if (uart == nullptr) {
return false;
}

const uint8_t num_read = uart->read(&u.buffer[buffer_used], ARRAY_SIZE(u.buffer)-buffer_used);
buffer_used += num_read;

if (buffer_used == 0) {
return false;
}
bool has_data = false;

uint32_t available = MAX(uart->available(), PACKET_SIZE*4);
while (available >= PACKET_SIZE) {
// ---------------
// Sync up with the header
const uint8_t header[] = {
0xEB, // Header MSB
0x90, // Header LSB
0x00, // Device ID
28 // length
};
for (uint8_t i = 0; i<ARRAY_SIZE(header); i++) {
available--;
if (uart->read() != header[i]) {
// header not aligned
continue;
}
}

if (u.packet.header_msb != PACKET_HEADER_MSB &&
!move_signature_in_buffer(1)) {
return false;
}
const uint8_t rest_of_packet_size = (PACKET_SIZE - ARRAY_SIZE(header));
if (available < rest_of_packet_size) {
// not enough data to read a full packet
return false;
}

if (buffer_used < sizeof(u.packet)) {
return false;
}
// ---------------
// header is aligned!
// ---------------

// sanity checks; see data sheet on these fixed values.
if (u.packet.header_lsb != PACKET_HEADER_LSB ||
u.packet.device_id != 0x00 ||
u.packet.length != 28 ||
u.calculate_checksum() != u.packet.checksum) {
// sanity checks failed - discard and try again next time we're called:
move_signature_in_buffer(1);
return false;
}
uint8_t buffer[rest_of_packet_size];
available -= uart->read(buffer, ARRAY_SIZE(buffer));

reading_m = be16toh(u.packet.object1_alt) * 0.01;

// consume this packet:
move_signature_in_buffer(sizeof(u.packet));

/* From datasheet:
Altitude data from radar may have unexpected
or incorrect readings if aircraft pitch and/or
roll are beyond the radar’s detection angle
(Azimuth 43°，Elevation 30°). The ideal case is
keeping radar perpendicular to ground.
*/
#ifndef HAL_BUILD_AP_PERIPH // we may need to move this elsewhere to the entire function or driver?
const auto &ahrs = AP::ahrs();
if (fabs(ahrs.roll) > radians(43)) {
return false;
}
if (fabs(ahrs.pitch) > radians(30)) {
return false;
}
#endif

snr = u.packet.object1_snr;

/* From datasheet:
Altitude measurements associated with a SNR value
of 13dB or lower are considered erroneous
*/
if (snr <= 13)
{
signal_quality_pct = RangeFinder::SIGNAL_QUALITY_MIN;
// out of range according to datasheet
// max valid altitude from device is 655 metres
if (snr == 0)
{
reading_m = MAX(656, max_distance_cm() * 0.01 + 1);
return true;
const uint8_t checksum = buffer[ARRAY_SIZE(buffer)-1]; // last byte is a checksum
if (crc_sum_of_bytes(buffer, ARRAY_SIZE(buffer)-1) != checksum) {
// bad Checksum
continue;
}

return false;
}

signal_quality_pct = RangeFinder::SIGNAL_QUALITY_MAX;
const uint8_t malfunction_alert = buffer[0];
reading_m = UINT16_VALUE(buffer[2], buffer[3]) * 0.01;
const uint8_t snr = buffer[4];

check_for_malfunction();
has_data = true;

return true;
}
if (malfunction_alert_prev != malfunction_alert) {
malfunction_alert_prev = malfunction_alert;
report_malfunction(malfunction_alert);
}

int8_t AP_RangeFinder_Ainstein_LR_D1::get_signal_quality_pct() const
{
return signal_quality_pct;
/* From datasheet:
Altitude measurements associated with a SNR value
of 13dB or lower are considered erroneous
*/
signal_quality_pct = (snr <= 13) ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX;

if (snr == 0) {
// out of range according to datasheet
// max valid altitude from device is 655 metres
reading_m = MAX(656, max_distance_cm() * 0.01 + 1);
has_data = false;
}
}

return has_data;
}

void AP_RangeFinder_Ainstein_LR_D1::check_for_malfunction() {
uint8_t malfunction_alert = u.packet.malfunction_alert;
if (malfunction_alert & static_cast<uint8_t>(MalfunctionAlert::Temperature)) {
void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_) {
if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Temperature)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert");
}
if (malfunction_alert & static_cast<uint8_t>(MalfunctionAlert::Voltage)) {
if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Voltage)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Voltage alert");
}
}
Expand Down
32 changes: 4 additions & 28 deletions libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,13 +45,9 @@ class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial

// 0 is no return value, 100 is perfect. false means signal
// quality is not available
int8_t get_signal_quality_pct() const override;
int8_t get_signal_quality_pct() const override { return signal_quality_pct; };

// find signature byte in buffer starting at start, moving that
// byte and following bytes to start of buffer.
bool move_signature_in_buffer(uint8_t start);

void check_for_malfunction();
static void report_malfunction(const uint8_t _malfunction_alert_);

enum class MalfunctionAlert {
Temperature = 0x01,
Expand All @@ -60,28 +56,8 @@ class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial
AltitudeReading = 0x80,
};

union LRD1Union {
struct PACKED LRD1Packet {
uint8_t header_msb;
uint8_t header_lsb;
uint8_t device_id;
uint8_t length; // "fixed as 28 bytes"
uint8_t malfunction_alert;
uint8_t objects_number; // "fixed as 1 (in version FW 6.0.7.x only), no longer reliable for data validation checks"
uint16_t object1_alt;
uint8_t object1_snr;
uint16_t object1_velocity;
uint8_t unused[20];
uint8_t checksum; // (data4+data5+…+data29+data31) bitwise-AND with 0xFF
} packet;
uint8_t buffer[64]; // each packet is 30 bytes long

// return checksum calculated from data in buffer
uint8_t calculate_checksum() const;
} u;
uint8_t buffer_used;

uint8_t snr = 255; // stashed SNR value after successful reading; 255 is unknown
static constexpr uint8_t PACKET_SIZE = 32;
uint8_t malfunction_alert_prev;
int8_t signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
};

Expand Down

0 comments on commit 3d5e4ff

Please sign in to comment.