diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
new file mode 100644
index 0000000000..0953f7ed02
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -0,0 +1,189 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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.h"
+#include "AP_Proximity_LightWareSF40C.h"
+
+extern const AP_HAL::HAL &hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_Proximity::var_info[] = {
+ // 0 is reserved for possible addition of an ENABLED parameter
+
+ // @Param: _TYPE
+ // @DisplayName: Proximity type
+ // @Description: What type of proximity sensor is connected
+ // @Values: 0:None,1:LightWareSF40C
+ // @User: Standard
+ AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0),
+
+ // @Param: _ORIENT
+ // @DisplayName: Proximity sensor orientation
+ // @Description: Proximity sensor orientation
+ // @Values: 0:Default,1:Upside Down
+ // @User: Standard
+ AP_GROUPINFO("_ORIENT", 2, AP_Proximity, _orientation[0], 0),
+
+ // @Param: _YAW_CORR
+ // @DisplayName: Proximity sensor yaw correction
+ // @Description: Proximity sensor yaw correction
+ // @Range: -180 180
+ // @User: Standard
+ AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], PROXIMITY_YAW_CORRECTION_DEFAULT),
+
+#if PROXIMITY_MAX_INSTANCES > 1
+ // @Param: 2_TYPE
+ // @DisplayName: Second Proximity type
+ // @Description: What type of proximity sensor is connected
+ // @Values: 0:None,1:LightWareSF40C
+ // @User: Advanced
+ AP_GROUPINFO("2_TYPE", 4, AP_Proximity, _type[1], 0),
+
+ // @Param: _ORIENT
+ // @DisplayName: Second Proximity sensor orientation
+ // @Description: Second Proximity sensor orientation
+ // @Values: 0:Default,1:Upside Down
+ // @User: Standard
+ AP_GROUPINFO("2_ORIENT", 5, AP_Proximity, _orientation[1], 0),
+
+ // @Param: _YAW_CORR
+ // @DisplayName: Second Proximity sensor yaw correction
+ // @Description: Second Proximity sensor yaw correction
+ // @Range: -180 180
+ // @User: Standard
+ AP_GROUPINFO("2_YAW_CORR", 6, AP_Proximity, _yaw_correction[1], PROXIMITY_YAW_CORRECTION_DEFAULT),
+#endif
+
+ AP_GROUPEND
+};
+
+AP_Proximity::AP_Proximity(AP_SerialManager &_serial_manager) :
+ primary_instance(0),
+ num_instances(0),
+ serial_manager(_serial_manager)
+{
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+// initialise the Proximity class. We do detection of attached sensors here
+// we don't allow for hot-plugging of sensors (i.e. reboot required)
+void AP_Proximity::init(void)
+{
+ if (num_instances != 0) {
+ // init called a 2nd time?
+ return;
+ }
+ for (uint8_t i=0; iupdate();
+ }
+ }
+
+ // work out primary instance - first sensor returning good data
+ for (int8_t i=num_instances-1; i>=0; i--) {
+ if (drivers[i] != NULL && (state[i].status == Proximity_Good)) {
+ primary_instance = i;
+ }
+ }
+}
+
+// return sensor orientation
+uint8_t AP_Proximity::get_orientation(uint8_t instance) const
+{
+ if (instance >= PROXIMITY_MAX_INSTANCES) {
+ return 0;
+ }
+
+ return _orientation[instance].get();
+}
+
+// return sensor yaw correction
+int16_t AP_Proximity::get_yaw_correction(uint8_t instance) const
+{
+ if (instance >= PROXIMITY_MAX_INSTANCES) {
+ return 0;
+ }
+
+ return _yaw_correction[instance].get();
+}
+
+// return sensor health
+AP_Proximity::Proximity_Status AP_Proximity::get_status(uint8_t instance) const
+{
+ // sanity check instance number
+ if (instance >= num_instances) {
+ return Proximity_NotConnected;
+ }
+
+ return state[instance].status;
+}
+
+AP_Proximity::Proximity_Status AP_Proximity::get_status() const
+{
+ return get_status(primary_instance);
+}
+
+// detect if an instance of a proximity sensor is connected.
+void AP_Proximity::detect_instance(uint8_t instance)
+{
+ uint8_t type = _type[instance];
+ if (type == Proximity_Type_SF40C) {
+ if (AP_Proximity_LightWareSF40C::detect(serial_manager)) {
+ state[instance].instance = instance;
+ drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], serial_manager);
+ return;
+ }
+ }
+}
+
+// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
+// returns true on successful read and places distance in distance
+bool AP_Proximity::get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const
+{
+ if ((drivers[instance] == NULL) || (_type[instance] == Proximity_Type_None)) {
+ return false;
+ }
+ // get distance from backend
+ return drivers[instance]->get_horizontal_distance(angle_deg, distance);
+}
+
+// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
+// returns true on successful read and places distance in distance
+bool AP_Proximity::get_horizontal_distance(float angle_deg, float &distance) const
+{
+ return get_horizontal_distance(primary_instance, angle_deg, distance);
+}
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
new file mode 100644
index 0000000000..293ab991fa
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -0,0 +1,96 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+ */
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+
+#define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform
+#define PROXIMITY_YAW_CORRECTION_DEFAULT 22 // default correction for sensor error in yaw
+
+class AP_Proximity_Backend;
+
+class AP_Proximity
+{
+public:
+ friend class AP_Proximity_Backend;
+
+ AP_Proximity(AP_SerialManager &_serial_manager);
+
+ // Proximity driver types
+ enum Proximity_Type {
+ Proximity_Type_None = 0,
+ Proximity_Type_SF40C = 1,
+ };
+
+ enum Proximity_Status {
+ Proximity_NotConnected = 0,
+ Proximity_NoData,
+ Proximity_Good
+ };
+
+ // detect and initialise any available rangefinders
+ void init(void);
+
+ // update state of all rangefinders. Should be called at high rate from main loop
+ void update(void);
+
+ // return sensor orientation and yaw correction
+ uint8_t get_orientation(uint8_t instance) const;
+ int16_t get_yaw_correction(uint8_t instance) const;
+
+ // return sensor health
+ Proximity_Status get_status(uint8_t instance) const;
+ Proximity_Status get_status() const;
+
+ // Return the number of range finder instances
+ uint8_t num_sensors(void) const {
+ return num_instances;
+ }
+
+ // get distance in meters in a particular direction in degrees (0 is forward, clockwise)
+ // returns true on successful read and places distance in distance
+ bool get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const;
+ bool get_horizontal_distance(float angle_deg, float &distance) const;
+
+ // The Proximity_State structure is filled in by the backend driver
+ struct Proximity_State {
+ uint8_t instance; // the instance number of this proximity sensor
+ enum Proximity_Status status; // sensor status
+ };
+
+ // parameter list
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+ Proximity_State state[PROXIMITY_MAX_INSTANCES];
+ AP_Proximity_Backend *drivers[PROXIMITY_MAX_INSTANCES];
+ uint8_t primary_instance:3;
+ uint8_t num_instances:3;
+ AP_SerialManager &serial_manager;
+
+ // parameters for all instances
+ AP_Int8 _type[PROXIMITY_MAX_INSTANCES];
+ AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES];
+ AP_Int16 _yaw_correction[PROXIMITY_MAX_INSTANCES];
+
+ void detect_instance(uint8_t instance);
+ void update_instance(uint8_t instance);
+};
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
new file mode 100644
index 0000000000..205950728d
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
@@ -0,0 +1,36 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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
+#include
+#include "AP_Proximity.h"
+#include "AP_Proximity_Backend.h"
+
+/*
+ base class constructor.
+ This incorporates initialisation as well.
+*/
+AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) :
+ frontend(_frontend),
+ state(_state)
+{
+}
+
+// set status and update valid count
+void AP_Proximity_Backend::set_status(AP_Proximity::Proximity_Status status)
+{
+ state.status = status;
+}
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h
new file mode 100644
index 0000000000..0879f46332
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_Backend.h
@@ -0,0 +1,46 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+ */
+#pragma once
+
+#include
+#include
+#include "AP_Proximity.h"
+
+class AP_Proximity_Backend
+{
+public:
+ // constructor. This incorporates initialisation as well.
+ AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+
+ // we declare a virtual destructor so that Proximity drivers can
+ // override with a custom destructor if need be
+ virtual ~AP_Proximity_Backend(void) {}
+
+ // update the state structure
+ virtual void update() = 0;
+
+ // get distance in meters in a particular direction in degrees (0 is forward, clockwise)
+ // returns true on successful read and places distance in distance
+ virtual bool get_horizontal_distance(float angle_deg, float &distance) const = 0;
+
+protected:
+
+ // set status and update valid_count
+ void set_status(AP_Proximity::Proximity_Status status);
+
+ AP_Proximity &frontend;
+ AP_Proximity::Proximity_State &state; // reference to this instances state
+};
diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
new file mode 100644
index 0000000000..546314264f
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
@@ -0,0 +1,421 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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
+#include "AP_Proximity_LightWareSF40C.h"
+#include
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+/*
+ The constructor also initialises the proximity sensor. Note that this
+ constructor is not called until detect() returns true, so we
+ already know that we should setup the proximity sensor
+*/
+AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend,
+ AP_Proximity::Proximity_State &_state,
+ AP_SerialManager &serial_manager) :
+ AP_Proximity_Backend(_frontend, _state)
+{
+ uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
+ if (uart != nullptr) {
+ uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0));
+ }
+}
+
+// detect if a Lightware proximity sensor is connected by looking for a configured serial port
+bool AP_Proximity_LightWareSF40C::detect(AP_SerialManager &serial_manager)
+{
+ return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr;
+}
+
+// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
+bool AP_Proximity_LightWareSF40C::get_horizontal_distance(float angle_deg, float &distance) const
+{
+ uint8_t sector;
+ if (convert_angle_to_sector(angle_deg, sector)) {
+ if (_distance_valid[sector]) {
+ distance = _distance[sector];
+ return true;
+ }
+ }
+ return false;
+}
+
+// update the state of the sensor
+void AP_Proximity_LightWareSF40C::update(void)
+{
+ if (uart == nullptr) {
+ return;
+ }
+
+ // initialise sensor if necessary
+ bool initialised = initialise();
+
+ // process incoming messages
+ check_for_reply();
+
+ // request new data from sensor
+ if (initialised) {
+ request_new_data();
+ }
+
+ // check for timeout and set health status
+ if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_SF40C_TIMEOUT_MS)) {
+ set_status(AP_Proximity::Proximity_NoData);
+ } else {
+ set_status(AP_Proximity::Proximity_Good);
+ }
+}
+
+// initialise sensor (returns true if sensor is succesfully initialised)
+bool AP_Proximity_LightWareSF40C::initialise()
+{
+ // set motor direction once per second
+ if (_motor_direction > 1) {
+ if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
+ set_motor_direction();
+ }
+ }
+ // set forward direction once per second
+ if (_forward_direction != frontend.get_yaw_correction(state.instance)) {
+ if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
+ set_forward_direction();
+ }
+ }
+ // request motors turn on once per second
+ if (_motor_speed == 0) {
+ if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
+ set_motor_speed(true);
+ }
+ return false;
+ }
+ return true;
+}
+
+// set speed of rotating motor
+void AP_Proximity_LightWareSF40C::set_motor_speed(bool on_off)
+{
+ // exit immediately if no uart
+ if (uart == nullptr) {
+ return;
+ }
+
+ // set motor update speed
+ if (on_off) {
+ uart->write("#MBS,3\r\n"); // send request to spin motor at 4.5hz
+ } else {
+ uart->write("#MBS,0\r\n"); // send request to stop motor
+ }
+
+ // request update motor speed
+ uart->write("?MBS\r\n");
+ _last_request_type = RequestType_MotorSpeed;
+ _last_request_ms = AP_HAL::millis();
+}
+
+// set spin direction of motor
+void AP_Proximity_LightWareSF40C::set_motor_direction()
+{
+ // exit immediately if no uart
+ if (uart == nullptr) {
+ return;
+ }
+
+ // set motor update speed
+ if (frontend.get_orientation(state.instance) == 0) {
+ uart->write("#MBD,0\r\n"); // spin clockwise
+ } else {
+ uart->write("#MBD,1\r\n"); // spin counter clockwise
+ }
+
+ // request update on motor direction
+ uart->write("?MBD\r\n");
+ _last_request_type = RequestType_MotorDirection;
+ _last_request_ms = AP_HAL::millis();
+}
+
+// set forward direction (to allow rotating lidar)
+void AP_Proximity_LightWareSF40C::set_forward_direction()
+{
+ // exit immediately if no uart
+ if (uart == nullptr) {
+ return;
+ }
+
+ // set forward direction
+ char request_str[15];
+ int16_t yaw_corr = frontend.get_yaw_correction(state.instance);
+ sprintf(request_str, "#MBF,%d\r\n", (int)yaw_corr);
+ uart->write(request_str);
+
+ // request update on motor direction
+ uart->write("?MBF\r\n");
+ _last_request_type = RequestType_ForwardDirection;
+ _last_request_ms = AP_HAL::millis();
+}
+
+// request new data if required
+void AP_Proximity_LightWareSF40C::request_new_data()
+{
+ if (uart == nullptr) {
+ return;
+ }
+
+ // after timeout assume no reply will ever come
+ uint32_t now = AP_HAL::millis();
+ if ((_last_request_type != RequestType_None) && ((now - _last_request_ms) > PROXIMITY_SF40C_TIMEOUT_MS)) {
+ _last_request_type = RequestType_None;
+ _last_request_ms = 0;
+ }
+
+ // if we are not waiting for a reply, ask for something
+ if (_last_request_type == RequestType_None) {
+ _request_count++;
+ if (_request_count >= 5) {
+ send_request_for_health();
+ _request_count = 0;
+ } else {
+ // request new distance measurement
+ send_request_for_distance();
+ }
+ _last_request_ms = now;
+ }
+}
+
+// send request for sensor health
+void AP_Proximity_LightWareSF40C::send_request_for_health()
+{
+ if (uart == nullptr) {
+ return;
+ }
+
+ uart->write("?GS\r\n");
+ _last_request_type = RequestType_Health;
+ _last_request_ms = AP_HAL::millis();
+}
+
+// send request for distance from the next sector
+bool AP_Proximity_LightWareSF40C::send_request_for_distance()
+{
+ if (uart == nullptr) {
+ return false;
+ }
+
+ // increment sector
+ _last_sector++;
+ if (_last_sector >= _num_sectors) {
+ _last_sector = 0;
+ }
+
+ // prepare request
+ char request_str[15];
+ sprintf(request_str, "?TS,%d,%d\r\n", (int)(_sector_width_deg[_last_sector]), (int)(_sector_middle_deg[_last_sector]));
+ uart->write(request_str);
+
+
+ // record request for distance
+ _last_request_type = RequestType_DistanceMeasurement;
+ _last_request_ms = AP_HAL::millis();
+
+ return true;
+}
+
+// check for replies from sensor, returns true if at least one message was processed
+bool AP_Proximity_LightWareSF40C::check_for_reply()
+{
+ if (uart == nullptr) {
+ return false;
+ }
+
+ // read any available lines from the lidar
+ // if CR (i.e. \r), LF (\n) it means we have received a full packet so send for processing
+ // lines starting with # are ignored because this is the echo of a set-motor request which has no reply
+ // lines starting with ? are the echo back of our distance request followed by the sensed distance
+ // distance data appears after a
+ // distance data is comma separated so we put into separate elements (i.e. angle,distance)
+ uint16_t count = 0;
+ int16_t nbytes = uart->available();
+ while (nbytes-- > 0) {
+ char c = uart->read();
+ // check for end of packet
+ if (c == '\r' || c == '\n') {
+ if ((element_len[0] > 0)) {
+ if (process_reply()) {
+ count++;
+ }
+ }
+ // clear buffers after processing
+ clear_buffers();
+ ignore_reply = false;
+ wait_for_space = false;
+
+ // if message starts with # ignore it
+ } else if (c == '#' || ignore_reply) {
+ ignore_reply = true;
+
+ // if waiting for
+ } else if (c == '?') {
+ wait_for_space = true;
+
+ } else if (wait_for_space) {
+ if (c == ' ') {
+ wait_for_space = false;
+ }
+
+ // if comma, move onto filling in 2nd element
+ } else if (c == ',') {
+ if ((element_num == 0) && (element_len[0] > 0)) {
+ element_num++;
+ } else {
+ // don't support 3rd element so clear buffers
+ clear_buffers();
+ ignore_reply = true;
+ }
+
+ // if part of a number, add to element buffer
+ } else if (isdigit(c) || c == '.' || c == '-') {
+ element_buf[element_num][element_len[element_num]] = c;
+ element_len[element_num]++;
+ if (element_len[element_num] >= sizeof(element_buf[element_num])-1) {
+ // too long, discard the line
+ clear_buffers();
+ ignore_reply = true;
+ }
+ }
+ }
+
+ return (count > 0);
+}
+
+// process reply
+bool AP_Proximity_LightWareSF40C::process_reply()
+{
+ if (uart == nullptr) {
+ return false;
+ }
+
+ bool success = false;
+
+ switch (_last_request_type) {
+ case RequestType_None:
+ break;
+
+ case RequestType_Health:
+ // expect result in the form "0xhhhh"
+ if (element_len[0] > 0) {
+ int result;
+ if (sscanf(element_buf[0], "%x", &result) > 0) {
+ _sensor_status.value = result;
+ success = true;
+ }
+ }
+ break;
+
+ case RequestType_MotorSpeed:
+ _motor_speed = atoi(element_buf[0]);
+ success = true;
+ break;
+
+ case RequestType_MotorDirection:
+ _motor_direction = atoi(element_buf[0]);
+ success = true;
+ break;
+
+ case RequestType_ForwardDirection:
+ _forward_direction = atoi(element_buf[0]);
+ success = true;
+ break;
+
+ case RequestType_DistanceMeasurement:
+ {
+ float angle_deg = (float)atof(element_buf[0]);
+ float distance_m = (float)atof(element_buf[1]);
+ uint8_t sector;
+ if (convert_angle_to_sector(angle_deg, sector)) {
+ _angle[sector] = angle_deg;
+ _distance[sector] = distance_m;
+ _distance_valid[sector] = true;
+ _last_distance_received_ms = AP_HAL::millis();
+ success = true;
+ }
+ break;
+ }
+
+ default:
+ break;
+ }
+
+ // mark request as cleared
+ if (success) {
+ _last_request_type = RequestType_None;
+ }
+
+ return success;
+}
+
+// clear buffers ahead of processing next message
+void AP_Proximity_LightWareSF40C::clear_buffers()
+{
+ element_len[0] = 0;
+ element_len[1] = 0;
+ element_num = 0;
+ memset(element_buf, 0, sizeof(element_buf));
+}
+
+bool AP_Proximity_LightWareSF40C::convert_angle_to_sector(float angle_degrees, uint8_t §or) const
+{
+ // sanity check angle
+ if (angle_degrees > 360.0f || angle_degrees < -180.0f) {
+ return false;
+ }
+
+ // convert to 0 ~ 360
+ if (angle_degrees < 0.0f) {
+ angle_degrees += 360.0f;
+ }
+
+ bool closest_found = false;
+ uint8_t closest_sector;
+ float closest_angle;
+
+ // search for which sector angle_degrees falls into
+ for (uint8_t i = 0; i < _num_sectors; i++) {
+ float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - angle_degrees));
+
+ // record if closest
+ if (!closest_found || angle_diff < closest_angle) {
+ closest_found = true;
+ closest_sector = i;
+ closest_angle = angle_diff;
+ }
+
+ if (fabsf(angle_diff) <= _sector_width_deg[i] / 2.0f) {
+ sector = i;
+ return true;
+ }
+ }
+
+ // angle_degrees might have been within a gap between sectors
+ if (closest_found) {
+ sector = closest_sector;
+ return true;
+ }
+
+ return false;
+}
diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h
new file mode 100644
index 0000000000..b9d804d64d
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h
@@ -0,0 +1,104 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
+
+#include "AP_Proximity.h"
+#include "AP_Proximity_Backend.h"
+
+#define PROXIMITY_SF40C_SECTORS_MAX 8 // maximum number of sectors
+#define PROXIMITY_SF40C_SECTOR_WIDTH_DEG (360/PROXIMITY_SF40C_SECTORS_MAX) // angular width of each sector
+#define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
+
+class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend
+{
+
+public:
+ // constructor
+ AP_Proximity_LightWareSF40C(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager);
+
+ // static detection function
+ static bool detect(AP_SerialManager &serial_manager);
+
+ // get distance in meters in a particular direction in degrees (0 is forward, clockwise)
+ // returns true on successful read and places distance in distance
+ bool get_horizontal_distance(float angle_deg, float &distance) const;
+
+ // update state
+ void update(void);
+
+private:
+
+ enum RequestType {
+ RequestType_None = 0,
+ RequestType_Health,
+ RequestType_MotorSpeed,
+ RequestType_MotorDirection,
+ RequestType_ForwardDirection,
+ RequestType_DistanceMeasurement
+ };
+
+ // initialise sensor (returns true if sensor is succesfully initialised)
+ bool initialise();
+ void set_motor_speed(bool on_off);
+ void set_motor_direction();
+ void set_forward_direction();
+
+ // send request for something from sensor
+ void request_new_data();
+ void send_request_for_health();
+ bool send_request_for_distance();
+
+ // check and process replies from sensor
+ bool check_for_reply();
+ bool process_reply();
+ void clear_buffers();
+ bool convert_angle_to_sector(float angle_degrees, uint8_t §or) const;
+
+ // reply related variables
+ AP_HAL::UARTDriver *uart = nullptr;
+ char element_buf[2][10];
+ uint8_t element_len[2];
+ uint8_t element_num;
+ bool ignore_reply; // true if we should ignore the incoming message (because it is just echoing our command)
+ bool wait_for_space; // space marks the start of returned data
+
+ // request related variables
+ enum RequestType _last_request_type; // last request made to sensor
+ uint8_t _last_sector; // last sector requested
+ uint32_t _last_request_ms; // system time of last request
+ uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
+ uint8_t _request_count; // counter used to interleave requests for distance with health requests
+
+ // sensor health register
+ union {
+ struct PACKED {
+ uint16_t motor_stopped : 1;
+ uint16_t motor_dir : 1; // 0 = clockwise, 1 = counter-clockwise
+ uint16_t motor_fault : 1;
+ uint16_t torque_control : 1; // 0 = automatic, 1 = manual
+ uint16_t laser_fault : 1;
+ uint16_t low_battery : 1;
+ uint16_t flat_battery : 1;
+ uint16_t system_restarting : 1;
+ uint16_t no_results_available : 1;
+ uint16_t power_saving : 1;
+ uint16_t user_flag1 : 1;
+ uint16_t user_flag2 : 1;
+ uint16_t unused1 : 1;
+ uint16_t unused2 : 1;
+ uint16_t spare_input : 1;
+ uint16_t major_system_abnormal : 1;
+ } _flags;
+ uint16_t value;
+ } _sensor_status;
+
+ // sensor data
+ uint8_t _motor_speed; // motor speed as reported by lidar
+ uint8_t _motor_direction = 99; // motor direction as reported by lidar
+ int16_t _forward_direction = 999; // forward direction as reported by lidar
+ uint8_t _num_sectors = PROXIMITY_SF40C_SECTORS_MAX; // number of sectors we will search
+ uint16_t _sector_middle_deg[PROXIMITY_SF40C_SECTORS_MAX] = {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector
+ uint8_t _sector_width_deg[PROXIMITY_SF40C_SECTORS_MAX] = {45, 45, 45, 45, 45, 45, 45, 45}; // width (in degrees) of each sector
+ float _angle[PROXIMITY_SF40C_SECTORS_MAX]; // angle to closest object within each sector
+ float _distance[PROXIMITY_SF40C_SECTORS_MAX]; // distance to closest object within each sector
+ bool _distance_valid[PROXIMITY_SF40C_SECTORS_MAX]; // true if a valid distance received for each sector
+};