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',
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',
diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp b/libraries/AP_LightWareSerial/AP_LightWareSerial.cpp
similarity index 75%
rename from libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp
rename to libraries/AP_LightWareSerial/AP_LightWareSerial.cpp
index 72f4c08329ef4..d688178c32a58 100644
--- a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp
+++ b/libraries/AP_LightWareSerial/AP_LightWareSerial.cpp
@@ -13,72 +13,72 @@
along with this program. If not, see .
*/
-#include "AP_Proximity_LightWareSerial.h"
+#include "AP_LightWareSerial.h"
+
+#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
-#if HAL_PROXIMITY_ENABLED
#include
-#include
#include
#include
#include
extern const AP_HAL::HAL& hal;
-#define PROXIMITY_LIGHTWARE_HEADER 0xAA
+#define 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)
+void AP_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)) {
+ if ((_uart_ref == nullptr) || (payload_len > LIGHTWARE_PAYLOAD_LEN_MAX)) {
return;
}
// check for sufficient space in outgoing buffer
- if (_uart->txspace() < payload_len + 6U) {
+ if (_uart_ref->txspace() < payload_len + 6U) {
return;
}
// write header
- _uart->write((uint8_t)PROXIMITY_LIGHTWARE_HEADER);
- uint16_t crc = crc_xmodem_update(0, PROXIMITY_LIGHTWARE_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->write(LOWBYTE(flags));
+ _uart_ref->write(LOWBYTE(flags));
crc = crc_xmodem_update(crc, LOWBYTE(flags));
- _uart->write(HIGHBYTE(flags));
+ _uart_ref->write(HIGHBYTE(flags));
crc = crc_xmodem_update(crc, HIGHBYTE(flags));
// msgid
- _uart->write(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->write(payload[i]);
+ _uart_ref->write(payload[i]);
crc = crc_xmodem_update(crc, payload[i]);
}
}
// checksum
- _uart->write(LOWBYTE(crc));
- _uart->write(HIGHBYTE(crc));
+ _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_Proximity_LightWareSerial::parse_byte(uint8_t b)
+bool AP_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");
+ 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 == PROXIMITY_LIGHTWARE_HEADER) {
+ if (b == LIGHTWARE_HEADER) {
_crc_expected = crc_xmodem_update(0, b);
_parse_state = ParseState::FLAGS_L;
}
@@ -94,7 +94,7 @@ bool AP_Proximity_LightWareSerial::parse_byte(uint8_t b)
_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)) {
+ if ((_msg.payload_len == 0) || (_msg.payload_len > LIGHTWARE_PAYLOAD_LEN_MAX)) {
// invalid payload length, abandon message
_parse_state = ParseState::HEADER;
} else {
@@ -141,4 +141,5 @@ bool AP_Proximity_LightWareSerial::parse_byte(uint8_t b)
return false;
}
-#endif // HAL_PROXIMITY_ENABLED
+#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
+
diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.h b/libraries/AP_LightWareSerial/AP_LightWareSerial.h
similarity index 64%
rename from libraries/AP_Proximity/AP_Proximity_LightWareSerial.h
rename to libraries/AP_LightWareSerial/AP_LightWareSerial.h
index f8da7dfa2630d..5b4861e2dc428 100644
--- a/libraries/AP_Proximity/AP_Proximity_LightWareSerial.h
+++ b/libraries/AP_LightWareSerial/AP_LightWareSerial.h
@@ -1,16 +1,20 @@
#pragma once
-#include "AP_Proximity_Backend_Serial.h"
+#include
+#include
-#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)
+#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
-class AP_Proximity_LightWareSerial : public AP_Proximity_Backend_Serial
-{
+#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:
- // constructor
- using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
+ AP_LightWareSerial(AP_HAL::UARTDriver *&uart_ref) :
+ _uart_ref(uart_ref)
+ {
+ }
protected:
@@ -42,11 +46,13 @@ class AP_Proximity_LightWareSerial : public AP_Proximity_Backend_Serial
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 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;
-};
-#endif // HAL_PROXIMITY_ENABLED
+private:
+ AP_HAL::UARTDriver *& _uart_ref;
+};
+#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
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_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