Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-02-06 06:52:44 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

AP_RangeFinder: create and use AP_RangeFinder_Backend_I2C

Browse Source
This commit is contained in:
Peter Barker
2025-10-27 11:54:56 +11:00
committed by Andrew Tridgell
parent 402ca1f881
commit 91b58f2f70
5 changed files with 72 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@@ -310,8 +310,11 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
}
FOREACH_I2C(i) {
auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
if (device_ptr == nullptr) {
continue;
}
if (_add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(state[instance], params[instance],
device_ptr),
*device_ptr),
instance)) {
break;
}

21
libraries/AP_RangeFinder/AP_RangeFinder_Backend_I2C.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "AP_RangeFinder_Backend_I2C.h"
#include "AP_RangeFinder_config.h"
#if AP_RANGEFINDER_ENABLED
AP_RangeFinder_Backend_I2C *AP_RangeFinder_Backend_I2C::configure(AP_RangeFinder_Backend_I2C *sensor)
{
if (sensor == nullptr) {
return nullptr;
}
if (!sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
#endif // AP_RANGEFINDER_ENABLED

34
libraries/AP_RangeFinder/AP_RangeFinder_Backend_I2C.h Normal file
View File

@@ -0,0 +1,34 @@
#pragma once
#include "AP_RangeFinder_config.h"
#if AP_RANGEFINDER_ENABLED
#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend.h"
#include <AP_HAL/I2CDevice.h>
class AP_RangeFinder_Backend_I2C : public AP_RangeFinder_Backend
{
public:
protected:
// constructor
AP_RangeFinder_Backend_I2C(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::I2CDevice &_dev)
: AP_RangeFinder_Backend(_state, _params)
, dev(_dev)
{ }
// configures the rangefinder, deletes it if unable to init it.
static AP_RangeFinder_Backend_I2C *configure(AP_RangeFinder_Backend_I2C *);
AP_HAL::I2CDevice &dev;
virtual bool init() = 0;
};
#endif // AP_RANGEFINDER_ENABLED

47
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@@ -30,50 +30,16 @@
extern const AP_HAL::HAL& hal;
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::I2CDevice *dev)
: AP_RangeFinder_Backend(_state, _params)
, _dev(dev)
{
}
/*
detect if a Maxbotix rangefinder is connected. We'll detect by
trying to take a reading on I2C. If we get a result the sensor is
there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::I2CDevice *dev)
bool AP_RangeFinder_MaxsonarI2CXL::init(void)
{
if (!dev) {
return nullptr;
}
AP_RangeFinder_MaxsonarI2CXL *sensor
= NEW_NOTHROW AP_RangeFinder_MaxsonarI2CXL(_state, _params, dev);
if (!sensor) {
return nullptr;
}
if (!sensor->_init()) {
delete sensor;
return nullptr;
}
return sensor;
}
/*
initialise sensor
*/
bool AP_RangeFinder_MaxsonarI2CXL::_init(void)
{
_dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(dev.get_semaphore());
if (!start_reading()) {
_dev->get_semaphore()->give();
return false;
}
@@ -82,13 +48,10 @@ bool AP_RangeFinder_MaxsonarI2CXL::_init(void)
uint16_t reading_cm;
if (!get_reading(reading_cm)) {
_dev->get_semaphore()->give();
return false;
}
_dev->get_semaphore()->give();
_dev->register_periodic_callback(100000,
dev.register_periodic_callback(100000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_MaxsonarI2CXL::_timer, void));
return true;
@@ -100,7 +63,7 @@ bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
uint8_t cmd = AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING;
// send command to take reading
return _dev->transfer(&cmd, sizeof(cmd), nullptr, 0);
return dev.transfer(&cmd, sizeof(cmd), nullptr, 0);
}
// read - return last value measured by sensor
@@ -109,7 +72,7 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
be16_t val;
// take range reading and read back results
bool ret = _dev->transfer(nullptr, 0, (uint8_t *) &val, sizeof(val));
bool ret = dev.transfer(nullptr, 0, (uint8_t *) &val, sizeof(val));
if (ret) {
// combine results into distance

18
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h
View File

@@ -5,20 +5,21 @@
#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED
#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend.h"
#include <AP_HAL/I2CDevice.h>
#include "AP_RangeFinder_Backend_I2C.h"
#define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70
#define AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING 0x51
class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend_I2C
{
public:
// static detection function
static AP_RangeFinder_Backend *detect(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::I2CDevice *dev);
class AP_HAL::I2CDevice &dev) {
// this will free the object if configuration fails:
return configure(NEW_NOTHROW AP_RangeFinder_MaxsonarI2CXL(_state, _params, dev));
}
// update state
void update(void) override;
@@ -31,11 +32,9 @@ protected:
private:
// constructor
AP_RangeFinder_MaxsonarI2CXL(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_HAL::I2CDevice *dev);
using AP_RangeFinder_Backend_I2C::AP_RangeFinder_Backend_I2C;
bool _init(void);
bool init(void) override;
void _timer(void);
uint16_t distance;
@@ -44,7 +43,6 @@ private:
// start a reading
bool start_reading(void);
bool get_reading(uint16_t &reading_cm);
AP_HAL::I2CDevice *_dev;
};
#endif // AP_RANGEFINDER_MAXSONARI2CXL_ENABLED