mirror of https://github.com/ArduPilot/ardupilot
AP_Proximity: add library and SF40C driver
This commit is contained in:
Randy Mackay
parent
cc0bfcddcb
commit
c3087edbe8
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
};
|
||||
|
|
@ -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 §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;
|
||||
}
|
||||
|
|
@ -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
|
||||
};
|
||||
Loading…
Reference in New Issue