mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_PX4: remove I2CDriver
I2CDevice now serves for the same purpose.
This commit is contained in:
parent
73bb371918
commit
2ef78051c1
|
@ -15,7 +15,6 @@
|
||||||
#include "AnalogIn.h"
|
#include "AnalogIn.h"
|
||||||
#include "Util.h"
|
#include "Util.h"
|
||||||
#include "GPIO.h"
|
#include "GPIO.h"
|
||||||
#include "I2CDriver.h"
|
|
||||||
#include "I2CDevice.h"
|
#include "I2CDevice.h"
|
||||||
|
|
||||||
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
||||||
|
@ -32,7 +31,6 @@
|
||||||
|
|
||||||
using namespace PX4;
|
using namespace PX4;
|
||||||
|
|
||||||
static PX4I2CDriver i2cDriver;
|
|
||||||
static Empty::SPIDeviceManager spiDeviceManager;
|
static Empty::SPIDeviceManager spiDeviceManager;
|
||||||
//static Empty::GPIO gpioDriver;
|
//static Empty::GPIO gpioDriver;
|
||||||
|
|
||||||
|
@ -86,9 +84,6 @@ HAL_PX4::HAL_PX4() :
|
||||||
&uartEDriver, /* uartE */
|
&uartEDriver, /* uartE */
|
||||||
&uartFDriver, /* uartF */
|
&uartFDriver, /* uartF */
|
||||||
&i2c_mgr_instance,
|
&i2c_mgr_instance,
|
||||||
&i2cDriver, /* i2c */
|
|
||||||
NULL, /* only one i2c */
|
|
||||||
NULL, /* only one i2c */
|
|
||||||
&spiDeviceManager, /* spi */
|
&spiDeviceManager, /* spi */
|
||||||
&analogIn, /* analogin */
|
&analogIn, /* analogin */
|
||||||
&storageDriver, /* storage */
|
&storageDriver, /* storage */
|
||||||
|
|
|
@ -1,70 +0,0 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
||||||
|
|
||||||
#include "I2CDriver.h"
|
|
||||||
#include "I2CWrapper.h"
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
|
||||||
|
|
||||||
using namespace PX4;
|
|
||||||
|
|
||||||
// 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 contiguous registers */
|
|
||||||
uint8_t PX4I2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
|
|
||||||
uint8_t len, uint8_t* data)
|
|
||||||
{
|
|
||||||
return write(addr, 1, ®) == 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, ®, 1, data, 1) ? 0:1;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* readRegister: read contiguous 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, ®, 1, data, len) ? 0:1;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD
|
|
|
@ -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 contiguous 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 contiguous 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;
|
|
||||||
};
|
|
Loading…
Reference in New Issue