AP_Proximity: add library and SF40C driver

This commit is contained in:
Randy Mackay 2016-08-15 15:17:15 +09:00
parent cc0bfcddcb
commit c3087edbe8
6 changed files with 892 additions and 0 deletions

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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; i<PROXIMITY_MAX_INSTANCES; i++) {
detect_instance(i);
if (drivers[i] != NULL) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
}
// initialise status
state[i].status = Proximity_NotConnected;
}
}
// update Proximity state for all instances. This should be called at a high rate by the main loop
void AP_Proximity::update(void)
{
for (uint8_t i=0; i<num_instances; i++) {
if (drivers[i] != NULL) {
if (_type[i] == Proximity_Type_None) {
// allow user to disable a proximity sensor at runtime
state[i].status = Proximity_NotConnected;
continue;
}
drivers[i]->update();
}
}
// 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);
}

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>
#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);
};

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#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;
}

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#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
};

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity_LightWareSF40C.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include <stdio.h>
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 <space>
// distance data is comma separated so we put into separate elements (i.e. <space>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 <space>
} 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 &sector) 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;
}

View File

@ -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 &sector) 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
};