aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder: add sensor type to backend

Browse Source
This commit is contained in:
Pierre Kancir 2017-05-29 19:02:51 +02:00 committed by Francisco Ferreira
parent ba5189440e
commit 8b38bd2e33
17 changed files with 47 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
View File

@ -40,7 +40,7 @@ volatile struct range *rangerpru;
already know that we should setup the rangefinder
*/
AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND)
{
}

2
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
View File

@ -64,7 +64,7 @@ static const uint16_t waveform_mode1[32] = {
AP_RangeFinder_Bebop::AP_RangeFinder_Bebop(RangeFinder &_ranger,
uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state),
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND),
_thread(new Linux::Thread(FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Bebop::_loop, void)))
{
_init();

2
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp
View File

@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {

2
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: AP_RangeFinder_Backend(_ranger, instance, _state)
: AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
, _dev(std::move(dev)) {}
/*

2
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -28,7 +28,7 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {

2
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp
View File

@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN)
{
last_update_ms = AP_HAL::millis();
distance_cm = 0;

2
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -36,7 +36,7 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
: AP_RangeFinder_Backend(_ranger, instance, _state)
: AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND)
, _dev(hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
{
}

2
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp
View File

@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {

2
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
View File

@ -44,7 +44,7 @@ extern "C" {
already know that we should setup the rangefinder
*/
AP_RangeFinder_PX4_PWM::AP_RangeFinder_PX4_PWM(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state),
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN),
_last_timestamp(0),
_last_pulse_time_ms(0),
_disable_time_ms(0),

2
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -47,7 +47,7 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
RangeFinder::RangeFinder_Type _rftype)
: AP_RangeFinder_Backend(_ranger, instance, _state)
: AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
, _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
, rftype(_rftype)
{

2
libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
View File

@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN)
{
source = hal.analogin->channel(ranger._pin[instance]);
if (source == nullptr) {

2
libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp
View File

@ -36,7 +36,7 @@ extern const AP_HAL::HAL& hal;
already know that we should setup the rangefinder
*/
AP_RangeFinder_trone::AP_RangeFinder_trone(uint8_t bus, RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
: AP_RangeFinder_Backend(_ranger, instance, _state)
: AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
, dev(hal.i2c_mgr->get_device(bus, TRONE_I2C_ADDR))
{
}

2
libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp
View File

@ -28,7 +28,7 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_RADAR)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0);
if (uart != nullptr) {

21
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -881,3 +881,24 @@ const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) co
}
return pos_offset_zero;
}
MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type(uint8_t instance) const {
// sanity check instance
if (instance >= RANGEFINDER_MAX_INSTANCES) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
if (drivers[instance] == nullptr || _type[instance] == RangeFinder_TYPE_NONE) {
return MAV_DISTANCE_SENSOR_UNKNOWN;
}
return drivers[instance]->get_sensor_type();
}
MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return get_sensor_type(i);
}
return MAV_DISTANCE_SENSOR_UNKNOWN;
}

4
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -155,6 +155,10 @@ public:
}
int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
MAV_DISTANCE_SENSOR get_sensor_type(uint8_t instance) const;
MAV_DISTANCE_SENSOR get_sensor_type_orient(enum Rotation orientation) const;
// query status
RangeFinder_Status status(uint8_t instance) const;
RangeFinder_Status status_orient(enum Rotation orientation) const;

5
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
View File

@ -24,9 +24,10 @@ extern const AP_HAL::HAL& hal;
base class constructor.
This incorporates initialisation as well.
*/
AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, MAV_DISTANCE_SENSOR _sensor_type) :
ranger(_ranger),
state(_state)
state(_state),
sensor_type(_sensor_type)
{
_sem = hal.util->new_semaphore();
}

7
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -22,7 +22,7 @@ class AP_RangeFinder_Backend
{
public:
// constructor. This incorporates initialisation as well.
AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, MAV_DISTANCE_SENSOR _sensor_type);
// we declare a virtual destructor so that RangeFinder drivers can
// override with a custom destructor if need be
@ -36,6 +36,10 @@ public:
return ranger._powersave_range > 0 && ranger.estimated_terrain_height > ranger._powersave_range;
}
MAV_DISTANCE_SENSOR get_sensor_type() const {
return sensor_type;
}
virtual void handle_msg(mavlink_message_t *msg) { return; }
protected:
@ -48,6 +52,7 @@ protected:
RangeFinder &ranger;
RangeFinder::RangeFinder_State &state;
MAV_DISTANCE_SENSOR sensor_type;
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;