From afb1213c425ac6ee7281f2406bc7eaebe31ca710 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 15 Oct 2025 23:03:38 -0400 Subject: [PATCH 1/5] AP_LightWareSerial: Move LightWareSerial base class from AP_Proximity --- .../AP_LightWareSerial/AP_LightWareSerial.cpp | 145 ++++++++++++++++++ .../AP_LightWareSerial/AP_LightWareSerial.h | 58 +++++++ 2 files changed, 203 insertions(+) create mode 100644 libraries/AP_LightWareSerial/AP_LightWareSerial.cpp create mode 100644 libraries/AP_LightWareSerial/AP_LightWareSerial.h diff --git a/libraries/AP_LightWareSerial/AP_LightWareSerial.cpp b/libraries/AP_LightWareSerial/AP_LightWareSerial.cpp new file mode 100644 index 0000000000000..d688178c32a58 --- /dev/null +++ b/libraries/AP_LightWareSerial/AP_LightWareSerial.cpp @@ -0,0 +1,145 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_LightWareSerial.h" + +#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define LIGHTWARE_HEADER 0xAA + +// send message to sensor +void AP_LightWareSerial::send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len) +{ + if ((_uart_ref == nullptr) || (payload_len > LIGHTWARE_PAYLOAD_LEN_MAX)) { + return; + } + + // check for sufficient space in outgoing buffer + if (_uart_ref->txspace() < payload_len + 6U) { + return; + } + + // write header + _uart_ref->write((uint8_t)LIGHTWARE_HEADER); + uint16_t crc = crc_xmodem_update(0, LIGHTWARE_HEADER); + + // write flags including payload length + const uint16_t flags = ((payload_len+1) << 6) | (write ? 0x01 : 0); + _uart_ref->write(LOWBYTE(flags)); + crc = crc_xmodem_update(crc, LOWBYTE(flags)); + _uart_ref->write(HIGHBYTE(flags)); + crc = crc_xmodem_update(crc, HIGHBYTE(flags)); + + // msgid + _uart_ref->write(msgid); + crc = crc_xmodem_update(crc, msgid); + + // payload + if ((payload_len > 0) && (payload != nullptr)) { + for (uint16_t i = 0; i < payload_len; i++) { + _uart_ref->write(payload[i]); + crc = crc_xmodem_update(crc, payload[i]); + } + } + + // checksum + _uart_ref->write(LOWBYTE(crc)); + _uart_ref->write(HIGHBYTE(crc)); +} + +// process one byte received on serial port +// returns true if a complete message has been received +// state is stored in _msg structure +bool AP_LightWareSerial::parse_byte(uint8_t b) +{ + // check that payload buffer is large enough + static_assert(ARRAY_SIZE(_msg.payload) == LIGHTWARE_PAYLOAD_LEN_MAX, "AP_LightWareSerial: check _msg.payload array size"); + + // process byte depending upon current state + switch (_parse_state) { + + case ParseState::HEADER: + if (b == LIGHTWARE_HEADER) { + _crc_expected = crc_xmodem_update(0, b); + _parse_state = ParseState::FLAGS_L; + } + break; + + case ParseState::FLAGS_L: + _msg.flags_low = b; + _crc_expected = crc_xmodem_update(_crc_expected, b); + _parse_state = ParseState::FLAGS_H; + break; + + case ParseState::FLAGS_H: + _msg.flags_high = b; + _crc_expected = crc_xmodem_update(_crc_expected, b); + _msg.payload_len = UINT16_VALUE(_msg.flags_high, _msg.flags_low) >> 6; + if ((_msg.payload_len == 0) || (_msg.payload_len > LIGHTWARE_PAYLOAD_LEN_MAX)) { + // invalid payload length, abandon message + _parse_state = ParseState::HEADER; + } else { + _parse_state = ParseState::MSG_ID; + } + break; + + case ParseState::MSG_ID: + _msg.msgid = b; + _crc_expected = crc_xmodem_update(_crc_expected, b); + if (_msg.payload_len > 1) { + _parse_state = ParseState::PAYLOAD; + } else { + _parse_state = ParseState::CRC_L; + } + _payload_recv = 0; + break; + + case ParseState::PAYLOAD: + if (_payload_recv < (_msg.payload_len - 1)) { + _msg.payload[_payload_recv] = b; + _payload_recv++; + _crc_expected = crc_xmodem_update(_crc_expected, b); + } + if (_payload_recv >= (_msg.payload_len - 1)) { + _parse_state = ParseState::CRC_L; + } + break; + + case ParseState::CRC_L: + _msg.crc_low = b; + _parse_state = ParseState::CRC_H; + break; + + case ParseState::CRC_H: + _parse_state = ParseState::HEADER; + _msg.crc_high = b; + if (_crc_expected == UINT16_VALUE(_msg.crc_high, _msg.crc_low)) { + return true; + } + break; + } + + return false; +} + +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + diff --git a/libraries/AP_LightWareSerial/AP_LightWareSerial.h b/libraries/AP_LightWareSerial/AP_LightWareSerial.h new file mode 100644 index 0000000000000..5b4861e2dc428 --- /dev/null +++ b/libraries/AP_LightWareSerial/AP_LightWareSerial.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + +#include +#define LIGHTWARE_PAYLOAD_LEN_MAX 256 // maximum payload size we can accept (in some configurations sensor may send as large as 1023) + +class AP_LightWareSerial +{ +public: + AP_LightWareSerial(AP_HAL::UARTDriver *&uart_ref) : + _uart_ref(uart_ref) + { + } + +protected: + + // initialise sensor + void initialise(); + + // send message to sensor + void send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len); + + // process one byte received on serial port + // returns true if a complete message has been received + // state is stored in _msg structure + bool parse_byte(uint8_t b); + + enum class ParseState { + HEADER = 0, + FLAGS_L, + FLAGS_H, + MSG_ID, + PAYLOAD, + CRC_L, + CRC_H + } _parse_state; // state of incoming message processing + uint16_t _payload_recv; // number of message's payload bytes received so far + uint16_t _crc_expected; // latest message's expected crc + + // structure holding latest message contents + struct { + uint8_t flags_low; // flags low byte + uint8_t flags_high; // flags high byte + uint16_t payload_len; // latest message payload length (1+ bytes in payload) + uint8_t payload[LIGHTWARE_PAYLOAD_LEN_MAX]; // payload + uint8_t msgid; // latest message's message id + uint8_t crc_low; // crc low byte + uint8_t crc_high; // crc high byte + } _msg; + +private: + AP_HAL::UARTDriver *& _uart_ref; +}; +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED From e04b62249f0bfe319260cf877aa7d1ede0e6db90 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 15 Oct 2025 23:04:14 -0400 Subject: [PATCH 2/5] ardupilotwaf: add AP_LightWareSerial library --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 2bdd771d758ed..0e1b4896c7dfb 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -54,6 +54,7 @@ 'AP_OpticalFlow', 'AP_Param', 'AP_Rally', + 'AP_LightWareSerial', 'AP_RangeFinder', 'AP_Scheduler', 'AP_SerialManager', From d7d081ce881b58f18fb409d666c4aac6b92d3714 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 15 Oct 2025 23:04:46 -0400 Subject: [PATCH 3/5] AP_Proximity: Remove LightWareSerial base class from AP_Proximity --- .../AP_Proximity_LightWareSF45B.h | 11 +- .../AP_Proximity_LightWareSerial.cpp | 144 ------------------ .../AP_Proximity_LightWareSerial.h | 52 ------- 3 files changed, 7 insertions(+), 200 deletions(-) delete mode 100644 libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp delete mode 100644 libraries/AP_Proximity/AP_Proximity_LightWareSerial.h diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h index 203c59bea6320..c1a1b7f15ab55 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h @@ -3,12 +3,12 @@ #include "AP_Proximity_config.h" #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED - -#include "AP_Proximity_LightWareSerial.h" +#include "AP_Proximity_Backend_Serial.h" +#include #include -class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial +class AP_Proximity_LightWareSF45B : public AP_Proximity_Backend_Serial, public AP_LightWareSerial { public: @@ -17,7 +17,10 @@ class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params, uint8_t serial_instance) : - AP_Proximity_LightWareSerial(_frontend, _state, _params, serial_instance) {} + AP_Proximity_Backend_Serial(_frontend, _state, _params, serial_instance), + AP_LightWareSerial(AP_Proximity_Backend_Serial::_uart) + { + } uint16_t rxspace() const override { return 1280; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp deleted file mode 100644 index 72f4c08329ef4..0000000000000 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include "AP_Proximity_LightWareSerial.h" - -#if HAL_PROXIMITY_ENABLED -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -#define PROXIMITY_LIGHTWARE_HEADER 0xAA - -// send message to sensor -void AP_Proximity_LightWareSerial::send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len) -{ - if ((_uart == nullptr) || (payload_len > PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX)) { - return; - } - - // check for sufficient space in outgoing buffer - if (_uart->txspace() < payload_len + 6U) { - return; - } - - // write header - _uart->write((uint8_t)PROXIMITY_LIGHTWARE_HEADER); - uint16_t crc = crc_xmodem_update(0, PROXIMITY_LIGHTWARE_HEADER); - - // write flags including payload length - const uint16_t flags = ((payload_len+1) << 6) | (write ? 0x01 : 0); - _uart->write(LOWBYTE(flags)); - crc = crc_xmodem_update(crc, LOWBYTE(flags)); - _uart->write(HIGHBYTE(flags)); - crc = crc_xmodem_update(crc, HIGHBYTE(flags)); - - // msgid - _uart->write(msgid); - crc = crc_xmodem_update(crc, msgid); - - // payload - if ((payload_len > 0) && (payload != nullptr)) { - for (uint16_t i = 0; i < payload_len; i++) { - _uart->write(payload[i]); - crc = crc_xmodem_update(crc, payload[i]); - } - } - - // checksum - _uart->write(LOWBYTE(crc)); - _uart->write(HIGHBYTE(crc)); -} - -// process one byte received on serial port -// returns true if a complete message has been received -// state is stored in _msg structure -bool AP_Proximity_LightWareSerial::parse_byte(uint8_t b) -{ - // check that payload buffer is large enough - static_assert(ARRAY_SIZE(_msg.payload) == PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX, "AP_Proximity_LightWareSerial: check _msg.payload array size"); - - // process byte depending upon current state - switch (_parse_state) { - - case ParseState::HEADER: - if (b == PROXIMITY_LIGHTWARE_HEADER) { - _crc_expected = crc_xmodem_update(0, b); - _parse_state = ParseState::FLAGS_L; - } - break; - - case ParseState::FLAGS_L: - _msg.flags_low = b; - _crc_expected = crc_xmodem_update(_crc_expected, b); - _parse_state = ParseState::FLAGS_H; - break; - - case ParseState::FLAGS_H: - _msg.flags_high = b; - _crc_expected = crc_xmodem_update(_crc_expected, b); - _msg.payload_len = UINT16_VALUE(_msg.flags_high, _msg.flags_low) >> 6; - if ((_msg.payload_len == 0) || (_msg.payload_len > PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX)) { - // invalid payload length, abandon message - _parse_state = ParseState::HEADER; - } else { - _parse_state = ParseState::MSG_ID; - } - break; - - case ParseState::MSG_ID: - _msg.msgid = b; - _crc_expected = crc_xmodem_update(_crc_expected, b); - if (_msg.payload_len > 1) { - _parse_state = ParseState::PAYLOAD; - } else { - _parse_state = ParseState::CRC_L; - } - _payload_recv = 0; - break; - - case ParseState::PAYLOAD: - if (_payload_recv < (_msg.payload_len - 1)) { - _msg.payload[_payload_recv] = b; - _payload_recv++; - _crc_expected = crc_xmodem_update(_crc_expected, b); - } - if (_payload_recv >= (_msg.payload_len - 1)) { - _parse_state = ParseState::CRC_L; - } - break; - - case ParseState::CRC_L: - _msg.crc_low = b; - _parse_state = ParseState::CRC_H; - break; - - case ParseState::CRC_H: - _parse_state = ParseState::HEADER; - _msg.crc_high = b; - if (_crc_expected == UINT16_VALUE(_msg.crc_high, _msg.crc_low)) { - return true; - } - break; - } - - return false; -} - -#endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.h b/libraries/AP_Proximity/AP_Proximity_LightWareSerial.h deleted file mode 100644 index f8da7dfa2630d..0000000000000 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include "AP_Proximity_Backend_Serial.h" - -#if HAL_PROXIMITY_ENABLED -#define PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX 256 // maximum payload size we can accept (in some configurations sensor may send as large as 1023) - -class AP_Proximity_LightWareSerial : public AP_Proximity_Backend_Serial -{ - -public: - // constructor - using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial; - -protected: - - // initialise sensor - void initialise(); - - // send message to sensor - void send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len); - - // process one byte received on serial port - // returns true if a complete message has been received - // state is stored in _msg structure - bool parse_byte(uint8_t b); - - enum class ParseState { - HEADER = 0, - FLAGS_L, - FLAGS_H, - MSG_ID, - PAYLOAD, - CRC_L, - CRC_H - } _parse_state; // state of incoming message processing - uint16_t _payload_recv; // number of message's payload bytes received so far - uint16_t _crc_expected; // latest message's expected crc - - // structure holding latest message contents - struct { - uint8_t flags_low; // flags low byte - uint8_t flags_high; // flags high byte - uint16_t payload_len; // latest message payload length (1+ bytes in payload) - uint8_t payload[PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX]; // payload - uint8_t msgid; // latest message's message id - uint8_t crc_low; // crc low byte - uint8_t crc_high; // crc high byte - } _msg; -}; - -#endif // HAL_PROXIMITY_ENABLED From aef8a775485dc20efb5cba4f6cf92630a6f2234a Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 15 Oct 2025 23:05:13 -0400 Subject: [PATCH 4/5] AP_RangeFinder: Add support for LightWare GRF250/500 --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 28 ++- libraries/AP_RangeFinder/AP_RangeFinder.h | 4 + .../AP_RangeFinder_LightWare_GRF.cpp | 235 ++++++++++++++++++ .../AP_RangeFinder_LightWare_GRF.h | 104 ++++++++ .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 1 + .../AP_RangeFinder/AP_RangeFinder_config.h | 4 + 6 files changed, 366 insertions(+), 10 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 0e611c65c7f65..e71e19e0421b3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -62,6 +62,7 @@ #include "AP_RangeFinder_JRE_Serial.h" #include "AP_RangeFinder_Ainstein_LR_D1.h" #include "AP_RangeFinder_RDS02UF.h" +#include "AP_RangeFinder_LightWare_GRF.h" #include #include @@ -80,7 +81,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]), #if RANGEFINDER_MAX_INSTANCES > 1 @@ -89,7 +90,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]), #endif @@ -99,7 +100,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]), #endif @@ -109,7 +110,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]), #endif @@ -119,7 +120,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]), #endif @@ -129,7 +130,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]), #endif @@ -139,7 +140,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]), #endif @@ -149,7 +150,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]), #endif @@ -159,7 +160,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]), #endif @@ -169,7 +170,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_LightWare_GRF.cpp AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]), #endif @@ -616,6 +617,13 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial serial_create_fn = AP_RangeFinder_RDS02UF::create; break; #endif + +#if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + case Type::LightWare_GRF: + serial_create_fn = AP_RangeFinder_LightWareGRF::create; + break; +#endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + case Type::NONE: break; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 62518cbb0ef8b..1bd4de797698d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -185,6 +185,10 @@ class RangeFinder #if AP_RANGEFINDER_HEXSOONRADAR_ENABLED HEXSOON_RADAR = 44, #endif +#if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + LightWare_GRF = 45, +#endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + #if AP_RANGEFINDER_SIM_ENABLED SIM = 100, #endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.cpp new file mode 100644 index 0000000000000..50e9171430033 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.cpp @@ -0,0 +1,235 @@ +#include "AP_RangeFinder_config.h" + +#if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + +#include "AP_RangeFinder_LightWare_GRF.h" +#include +#include +#include + +#define GRF_UPDATE_RATE_HZ 50 +#define GRF_STREAM_CM_DISTANCES 5 +#define GRF_MAX_DISTANCE_CM 50000 + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_RangeFinder_LightWareGRF::var_info[] = { + // @Param: GRF_RET + // @DisplayName: LightWare GRF Distance Return Type + // @Description: Selects which single return to use. + // @Values: 0:FirstRaw,1:FirstFiltered,2:LastRaw,3:LastFiltered + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("GRF_RET", 12, AP_RangeFinder_LightWareGRF, return_selection, (uint8_t)GRF_ReturnSelection::FIRST_RAW), + + // @Param: GRF_ST + // @DisplayName: LightWare GRF Minimum Return Strength + // @Description: Minimum acceptable return signal strength in dB. Returns weaker than this will be ignored. Set to 0 to disable filtering. + // @Range: 0 255 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("GRF_ST", 13, AP_RangeFinder_LightWareGRF, minimum_return_strength, 0), + + // @Param: GRF_RATE + // @DisplayName: LightWare GRF Update Rate + // @Description: The update rate of the sensor in Hz. Must match the + // @Range: 1 50 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("GRF_RATE", 14, AP_RangeFinder_LightWareGRF, update_rate, GRF_UPDATE_RATE_HZ), + + AP_GROUPEND +}; + +AP_RangeFinder_LightWareGRF::AP_RangeFinder_LightWareGRF(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) + : AP_RangeFinder_Backend_Serial(_state, _params), + AP_LightWareSerial(AP_RangeFinder_Backend_Serial::uart) +{ + AP_Param::setup_object_defaults(this, var_info); + state.var_info = var_info; +} + +// Checks if PRODUCT_NAME payload matches expected GRF signature +bool AP_RangeFinder_LightWareGRF::matches_product_name(const uint8_t *buf, const uint16_t len) +{ + // Must be at least "GRFXXX\0" = 7 bytes + if (len < 7) { + return false; + } + + const char expected[] = "GRF"; + for (uint8_t i = 0; i < 3; i++) { // Only compare "GRF" for compatibility with GRF250, GRF500 + if (buf[i] != expected[i]) { + return false; + } + } + return true; +} + +// Parses config responses and advances setup step +void AP_RangeFinder_LightWareGRF::check_config(const MessageID &resp_cmd_id, const uint8_t* response_buf, const uint16_t& response_len) +{ + // GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "GRF: received cmd %d (expecting %d)", (uint8_t)resp_cmd_id, (uint8_t)config_step); + bool valid = false; + + switch (config_step) { + case ConfigStep::HANDSHAKE: + valid = (resp_cmd_id == MessageID::PRODUCT_NAME && + matches_product_name(response_buf, response_len)); + if (valid) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LightWare %s detected", (const char*)response_buf); + } + break; + + case ConfigStep::UPDATE_RATE: + if (resp_cmd_id == MessageID::UPDATE_RATE && response_len >= 4) { + const uint8_t response_update_rate = (uint8_t)UINT32_VALUE(response_buf[3], response_buf[2], response_buf[1], response_buf[0]); + valid = (response_update_rate == update_rate); + } + break; + + case ConfigStep::DISTANCE_OUTPUT: + valid = (resp_cmd_id == MessageID::DISTANCE_OUTPUT); + break; + + case ConfigStep::STREAM: + valid = (resp_cmd_id == MessageID::STREAM); + break; + + case ConfigStep::DONE: + break; + } + + if (valid && config_step != ConfigStep::DONE) { + config_step = static_cast(uint8_t(config_step) + 1); + return; + } +} + +// Send configuration messages to the rangefinder +void AP_RangeFinder_LightWareGRF::configure_rangefinder() +{ + if (config_step == ConfigStep::DONE) { + return; + } + + const uint32_t now = AP_HAL::millis(); + if (now - last_config_message_ms < 100) { + return; + } + + switch (config_step) { + + case ConfigStep::HANDSHAKE: + uart->write((uint8_t*)"UUU", 3); // Try to switch GRF sensor to serial mode + send_message((uint8_t)MessageID::PRODUCT_NAME, false, nullptr, 0); + break; + + case ConfigStep::UPDATE_RATE: { + const uint8_t payload[4] = {(uint8_t)update_rate, 0, 0, 0}; + send_message((uint8_t)MessageID::UPDATE_RATE, true, payload, 4); + break; + } + + case ConfigStep::DISTANCE_OUTPUT: { + uint8_t data_bit = 1, strength_bit = 2; + switch (GRF_ReturnSelection(return_selection.get())) { + case GRF_ReturnSelection::FIRST_RAW: + data_bit = 0; + break; + case GRF_ReturnSelection::LAST_RAW: + data_bit = 3; + strength_bit = 5; + break; + case GRF_ReturnSelection::LAST_FILTERED: + data_bit = 4; + strength_bit = 5; + break; + case GRF_ReturnSelection::FIRST_FILTERED: + break; + } + const uint8_t payload[4] = { + static_cast((1U << data_bit) | (1U << strength_bit)), 0, 0, 0 + }; + send_message((uint8_t)MessageID::DISTANCE_OUTPUT, true, payload, 4); + break; + } + + case ConfigStep::STREAM: { + const uint8_t payload[4] = {GRF_STREAM_CM_DISTANCES, 0, 0, 0}; + send_message((uint8_t)MessageID::STREAM, true, payload, 4); + break; + } + + case ConfigStep::DONE: + break; + } + + last_config_message_ms = now; +} + +// Processes the latest message held in the _msg structure +void AP_RangeFinder_LightWareGRF::process_message(float &sum_m, uint8_t &count) +{ + MessageID cmd_id = (MessageID)_msg.msgid; + + if (config_step != ConfigStep::DONE) { + // sensor configuration in progress + check_config(cmd_id, _msg.payload, _msg.payload_len); + return; + } + + if (cmd_id != MessageID::DISTANCE_DATA_CM || _msg.payload_len < 8) { + // beyond the configuration steps we only expect distance data messages + return; + } + + // Extract distance and strength + uint32_t distance_cm = UINT32_VALUE(_msg.payload[3], _msg.payload[2], _msg.payload[1], _msg.payload[0]) * 10; + uint32_t strength_db = UINT32_VALUE(_msg.payload[7], _msg.payload[6], _msg.payload[5], _msg.payload[4]); + + if (distance_cm == 0 || distance_cm > GRF_MAX_DISTANCE_CM) { + // out of range reading + return; + } + + if (minimum_return_strength == 0 || (int8_t)strength_db >= minimum_return_strength) { + float dist_m = distance_cm * 0.01f; + sum_m += dist_m; + count++; + } +} + +// Called periodically to fetch a new range reading +bool AP_RangeFinder_LightWareGRF::get_reading(float &reading_m) +{ + if (uart == nullptr) { + return false; + } + + if (config_step != ConfigStep::DONE) { + configure_rangefinder(); + } + + float sum_m = 0.0f; + uint8_t count = 0; + + // process up to 1K of characters per iteration + uint32_t nbytes = MIN(uart->available(), 1024U); + while (nbytes-- > 0) { + uint8_t c; + if (!uart->read(c)) { + continue; + } + if (parse_byte(c)) { + process_message(sum_m, count); + } + } + if (count > 0) { + reading_m = sum_m / count; + return true; + } + return false; +} + +#endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.h new file mode 100644 index 0000000000000..12f61e40d1707 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWare_GRF.h @@ -0,0 +1,104 @@ +#pragma once + +#include +#include + +#if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED + +class AP_RangeFinder_LightWareGRF : public AP_RangeFinder_Backend_Serial, AP_LightWareSerial { +public: + static AP_RangeFinder_Backend_Serial *create( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params) + { + return NEW_NOTHROW AP_RangeFinder_LightWareGRF(_state, _params); + } + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // Returns the MAVLink distance sensor type + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_LASER; } + + // Called periodically to fetch a new range reading + bool get_reading(float &reading_m) override; + + // Returns read timeout in milliseconds + uint16_t read_timeout_ms() const override { return 500; } + +private: + // Constructor + AP_RangeFinder_LightWareGRF(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params); + + // Supported command/message IDs by the GRF LightWare rangefinders. We don't use all of them yet. + enum class MessageID : uint8_t { + PRODUCT_NAME = 0, + HARDWARE_VERSION = 1, + FIRMWARE_VERSION = 2, + SERIAL_NUMBER = 3, + SAVE_PARAMETERS = 12, + RESET = 14, + DISTANCE_OUTPUT = 27, + STREAM = 30, + DISTANCE_DATA_CM = 44, + DISTANCE_DATA_MM = 45, + LASER_FIRING = 50, + TEMPERATURE = 55, + AUTO_EXPOSURE = 70, + UPDATE_RATE = 74, + ALARM_STATUS = 76, + RETURN_MODE = 77, + LOST_SIGNAL_COUNTER = 78, + MEDIAN_FILTER_ENABLE = 86, + MEDIAN_FILTER_SIZE = 87, + SMOOTHING_FILTER_ENABLE= 88, + SMOOTHING_FACTOR = 89, + BAUD_RATE = 91, + I2C_ADDRESS = 92, + ROLLING_AVERAGE_ENABLE = 93, + ROLLING_AVERAGE_SIZE = 94, + SLEEP_COMMAND = 98, + LED_STATE = 110, + ZERO_OFFSET = 114 + }; + + // Distance return modes + enum class GRF_ReturnSelection : uint8_t { + FIRST_RAW = 0, + FIRST_FILTERED = 1, + LAST_RAW = 2, + LAST_FILTERED = 3 + }; + + // Initialization configuration steps + enum class ConfigStep : uint8_t { + HANDSHAKE, + UPDATE_RATE, + DISTANCE_OUTPUT, + STREAM, + DONE + }; + + // Send configuration messages to the rangefinder + void configure_rangefinder(); + + // Parses config responses and advances setup step + void check_config(const MessageID &resp_cmd_id, const uint8_t* response_buf, const uint16_t& response_len); + + // Checks if PRODUCT_NAME payload matches expected GRF signature + bool matches_product_name(const uint8_t *buf, const uint16_t len); + + // Processes the latest message held in the _msg structure + void process_message(float &sum_m, uint8_t &count); + + uint32_t last_config_message_ms; // last time we sent an config message + ConfigStep config_step; // current configuration step + + AP_Int8 return_selection; // first or last return, filtered or unfiltered + AP_Int8 minimum_return_strength; // minimum acceptable signal strength in db + AP_Int8 update_rate; // update rate in Hz +}; + +#endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 86c88122e6c7a..9e868fe941ec9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -59,6 +59,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Values: 42:Ainstein_LR_D1 // @Values: 43:RDS02UF // @Values: 44:HexsoonRadar + // @vALUES: 45:LightWare-GRF // @Values: 100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index 78a8db76ceb49..185f863aaf06f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -91,6 +91,10 @@ #define AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED +#define AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_RANGEFINDER_LUA_ENABLED #define AP_RANGEFINDER_LUA_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED #endif From 468b162705a96a89108de6aa89116e76b862e539 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 15 Oct 2025 23:21:53 -0400 Subject: [PATCH 5/5] AP_Periph: Include AP_LightWareSerial library --- Tools/AP_Periph/wscript | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 4580238eaaa98..df1edc8cc04a9 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -55,6 +55,7 @@ def build(bld): 'Filter', 'AP_InternalError', 'AP_Airspeed', + 'AP_LightWareSerial', 'AP_RangeFinder', 'AP_ROMFS', 'AP_TemperatureSensor',