Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_PX4: use new I2CDevice interface

This commit is contained in:
Luiz Ywata 2016-05-04 16:51:24 -03:00 committed by Lucas De Marchi
parent f04b954a1c
commit f767918e0e
6 changed files with 167 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
View File

@ -12,7 +12,8 @@ namespace PX4 {
class PX4GPIO;
class PX4DigitalSource;
class NSHShellStream;
class PX4I2CDriver;
class I2CDevice;
class I2CDeviceManager;
class PX4_I2C;
class Semaphore;
}

11
libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
View File

@ -15,7 +15,7 @@
#include "AnalogIn.h"
#include "Util.h"
#include "GPIO.h"
#include "I2CDriver.h"
#include "I2CDevice.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
@ -31,7 +31,6 @@
using namespace PX4;
static PX4I2CDriver i2cDriver;
static Empty::SPIDeviceManager spiDeviceManager;
//static Empty::GPIO gpioDriver;
@ -43,7 +42,7 @@ static PX4AnalogIn analogIn;
static PX4Util utilInstance;
static PX4GPIO gpioDriver;
static Empty::I2CDeviceManager i2c_mgr_instance;
static I2CDeviceManager i2c_mgr_instance;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
@ -85,9 +84,9 @@ HAL_PX4::HAL_PX4() :
&uartEDriver, /* uartE */
&uartFDriver, /* uartF */
&i2c_mgr_instance,
&i2cDriver, /* i2c */
NULL, /* only one i2c */
NULL, /* only one i2c */
NULL, /* use i2c_mgr_instance */
NULL, /* use i2c_mgr_instance */
NULL, /* use i2c_mgr_instance */
&spiDeviceManager, /* spi */
&analogIn, /* analogin */
&storageDriver, /* storage */

73
libraries/AP_HAL_PX4/I2CDevice.cpp Normal file
View File

@ -0,0 +1,73 @@
#include "I2CDevice.h"
#include <AP_HAL/AP_HAL.h>
namespace PX4 {
class PX4_I2C : public device::I2C {
public:
PX4_I2C(uint8_t bus);
bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len);
};
PX4_I2C::PX4_I2C(uint8_t bus) :
I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL)
{
init();
}
bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
set_address(address);
return transfer(send, send_len, recv, recv_len) == OK;
}
bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
return _device.do_transfer(_address, send, send_len, recv, recv_len);
}
bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times)
{
while (times > 0) {
if (!transfer(nullptr, 0, recv, recv_len)) {
return false;
}
recv += recv_len;
times--;
}
return true;
}
int I2CDevice::get_fd()
{
return -1;
}
AP_HAL::Semaphore *I2CDevice::get_semaphore()
{
return &semaphore;
}
I2CDevice::~I2CDevice()
{
}
I2CDeviceManager::I2CDeviceManager()
{
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice>
I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
{
AP_HAL::OwnPtr<PX4_I2C> i2c { new PX4_I2C(bus) };
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(*i2c, address));
return dev;
}
}

87
libraries/AP_HAL_PX4/I2CDevice.h Normal file
View File

@ -0,0 +1,87 @@
#pragma once
#include <vector>
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include <drivers/device/i2c.h>
#include <arch/board/board.h>
#include "AP_HAL_PX4.h"
#include "board_config.h"
namespace PX4 {
class I2CDevice : public AP_HAL::I2CDevice {
public:
static I2CDevice *from(AP_HAL::I2CDevice *dev)
{
return static_cast<I2CDevice*>(dev);
}
/* AP_HAL::I2CDevice implementation */
I2CDevice(PX4_I2C &device, uint8_t address)
: _device(device)
, _address(address)
{
}
~I2CDevice();
/* See AP_HAL::I2CDevice::set_address() */
void set_address(uint8_t address) override { _address = address; }
/* See AP_HAL::I2CDevice::set_retries() */
void set_retries(uint8_t retries) override { _retries = retries; }
/* AP_HAL::Device implementation */
/* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
bool set_speed(enum Device::Speed speed) override { return true; }
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times) override;
/* See AP_HAL::Device::get_semaphore() */
AP_HAL::Semaphore *get_semaphore() override;
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle *register_periodic_callback(uint32_t period_usec, AP_HAL::MemberProc) override
{
/* Not implemented yet */
return nullptr;
};
/* See AP_HAL::Device::get_fd() */
int get_fd() override;
protected:
PX4_I2C &_device;
uint8_t _address;
uint8_t _retries = 0;
private:
// we use an empty semaphore as the underlying I2C class already has a semaphore
Empty::Semaphore semaphore;
};
class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
public:
friend class I2CDevice;
static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
{
return static_cast<I2CDeviceManager*>(i2c_mgr);
}
I2CDeviceManager();
/* AP_HAL::I2CDeviceManager implementation */
AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address) override;
};
}

100
libraries/AP_HAL_PX4/I2CDriver.cpp
View File

@ -1,100 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include "I2CDriver.h"
#include <drivers/device/i2c.h>
#include <arch/board/board.h>
#include "board_config.h"
extern const AP_HAL::HAL& hal;
using namespace PX4;
/*
wrapper class for I2C to expose protected functions from PX4Firmware
*/
class PX4::PX4_I2C : public device::I2C {
public:
PX4_I2C(uint8_t bus);
bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
};
// constructor
PX4_I2C::PX4_I2C(uint8_t bus) :
I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL)
{
/*
attach to the bus. Return value can be ignored as we have no probe function
*/
init();
}
/*
main transfer function
*/
bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
{
set_address(address);
return transfer(send, send_len, recv, recv_len) == OK;
}
// constructor for main i2c class
PX4I2CDriver::PX4I2CDriver(void)
{
px4_i2c = new PX4_I2C(PX4_I2C_BUS_EXPANSION);
if (px4_i2c == nullptr) {
AP_HAL::panic("Unable to allocate PX4 I2C driver");
}
}
/*
expose transfer function
*/
bool PX4I2CDriver::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
{
return px4_i2c->do_transfer(address, send, send_len, recv, recv_len);
}
/* write: for i2c devices which do not obey register conventions */
uint8_t PX4I2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
{
return do_transfer(addr, data, len, nullptr, 0) ? 0:1;
}
/* writeRegister: write a single 8-bit value to a register */
uint8_t PX4I2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
{
uint8_t d[2] = { reg, val };
return do_transfer(addr, d, sizeof(d), nullptr, 0) ? 0:1;
}
/* writeRegisters: write bytes to contigious registers */
uint8_t PX4I2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t* data)
{
return write(addr, 1, &reg) == 0 && write(addr, len, data) == 0;
}
/* read: for i2c devices which do not obey register conventions */
uint8_t PX4I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
{
return do_transfer(addr, nullptr, 0, data, len) ? 0:1;
}
/* readRegister: read from a device register - writes the register,
* then reads back an 8-bit value. */
uint8_t PX4I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
{
return do_transfer(addr, &reg, 1, data, 1) ? 0:1;
}
/* readRegister: read contigious device registers - writes the first
* register, then reads back multiple bytes */
uint8_t PX4I2CDriver::readRegisters(uint8_t addr, uint8_t reg,
uint8_t len, uint8_t* data)
{
return do_transfer(addr, &reg, 1, data, len) ? 0:1;
}
#endif // CONFIG_HAL_BOARD

47
libraries/AP_HAL_PX4/I2CDriver.h
View File

@ -1,47 +0,0 @@
#pragma once
#include "AP_HAL_PX4.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
class PX4::PX4I2CDriver : public AP_HAL::I2CDriver {
public:
PX4I2CDriver(void);
void begin() override {}
void end() override {}
void setTimeout(uint16_t ms) override {}
void setHighSpeed(bool active) override {}
// main transfer function
bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) override;
/* write: for i2c devices which do not obey register conventions */
uint8_t write(uint8_t addr, uint8_t len, uint8_t* data) override;
/* writeRegister: write a single 8-bit value to a register */
uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val) override;
/* writeRegisters: write bytes to contigious registers */
uint8_t writeRegisters(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* data) override;
/* read: for i2c devices which do not obey register conventions */
uint8_t read(uint8_t addr, uint8_t len, uint8_t* data) override;
/* readRegister: read from a device register - writes the register,
* then reads back an 8-bit value. */
uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data) override;
/* readRegister: read contigious device registers - writes the first
* register, then reads back multiple bytes */
uint8_t readRegisters(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* data) override;
uint8_t lockup_count() override { return 0; }
AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
private:
// we use an empty semaphore as the underlying I2C class already has a semaphore
Empty::Semaphore semaphore;
PX4_I2C *px4_i2c = nullptr;
};