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