AP_RangeFinder: Add GRF250/GRF500 serial driver

Tools/AP_Periph/wscript

@@ -55,6 +55,7 @@ def build(bld):
                      'Filter',
                      'AP_InternalError',
                      'AP_Airspeed',
+                     'AP_LightWareSerial',
                      'AP_RangeFinder',
                      'AP_ROMFS',
                      'AP_TemperatureSensor',

Tools/ardupilotwaf/ardupilotwaf.py

@@ -54,6 +54,7 @@
     'AP_OpticalFlow',
     'AP_Param',
     'AP_Rally',
+    'AP_LightWareSerial',
     'AP_RangeFinder',
     'AP_Scheduler',
     'AP_SerialManager',

libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp → libraries/AP_LightWareSerial/AP_LightWareSerial.cpp

@@ -13,72 +13,72 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#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 <AP_Common/AP_Common.h>
-#include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/utility/sparse-endian.h>
 #include <AP_Math/crc.h>
 #include <GCS_MAVLink/GCS.h>
 
 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
+

libraries/AP_Proximity/AP_Proximity_LightWareSerial.h → libraries/AP_LightWareSerial/AP_LightWareSerial.h

@@ -1,16 +1,20 @@
 #pragma once
 
-#include "AP_Proximity_Backend_Serial.h"
+#include <AP_RangeFinder/AP_RangeFinder_config.h>
+#include <AP_Proximity/AP_Proximity_config.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)
+#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED ||  AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
 
-class AP_Proximity_LightWareSerial : public AP_Proximity_Backend_Serial
-{
+#include <AP_HAL/AP_HAL.h>
+#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

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 <AP_LightWareSerial/AP_LightWareSerial.h>
 
 #include <Filter/Filter.h>
 
-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;

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 <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -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;
     }

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