forked from Archive/PX4-Autopilot
Build the px4io interfaces on top of the Device direct-access API.
This commit is contained in:
px4dev
parent
26eb0b9d72
commit
4f0ae1cdea
|
|
@ -85,7 +85,9 @@
|
|||
#include <modules/px4iofirmware/protocol.h>
|
||||
|
||||
#include "uploader.h"
|
||||
#include "interface.h"
|
||||
|
||||
extern device::Device *PX4IO_i2c_interface() weak_function;
|
||||
extern device::Device *PX4IO_serial_interface() weak_function;
|
||||
|
||||
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
|
||||
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
|
||||
|
|
@ -93,7 +95,7 @@
|
|||
class PX4IO : public device::CDev
|
||||
{
|
||||
public:
|
||||
PX4IO(PX4IO_interface *interface);
|
||||
PX4IO(device::Device *interface);
|
||||
virtual ~PX4IO();
|
||||
|
||||
virtual int init();
|
||||
|
|
@ -131,7 +133,7 @@ public:
|
|||
void print_status();
|
||||
|
||||
private:
|
||||
PX4IO_interface *_interface;
|
||||
device::Device *_interface;
|
||||
|
||||
// XXX
|
||||
unsigned _hardware;
|
||||
|
|
@ -316,7 +318,7 @@ PX4IO *g_dev;
|
|||
|
||||
}
|
||||
|
||||
PX4IO::PX4IO(PX4IO_interface *interface) :
|
||||
PX4IO::PX4IO(device::Device *interface) :
|
||||
CDev("px4io", "/dev/px4io"),
|
||||
_interface(interface),
|
||||
_hardware(0),
|
||||
|
|
@ -1129,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
|
|||
return -EINVAL;
|
||||
}
|
||||
|
||||
int ret = _interface->set_reg(page, offset, values, num_values);
|
||||
int ret = _interface->write((page << 8) | offset, (void *)values, num_values);
|
||||
if (ret != OK)
|
||||
debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno);
|
||||
return ret;
|
||||
|
|
@ -1150,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
|
|||
return -EINVAL;
|
||||
}
|
||||
|
||||
int ret = _interface->get_reg(page, offset, values, num_values);
|
||||
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
|
||||
if (ret != OK)
|
||||
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno);
|
||||
return ret;
|
||||
|
|
@ -1602,12 +1604,12 @@ start(int argc, char *argv[])
|
|||
if (g_dev != nullptr)
|
||||
errx(1, "already loaded");
|
||||
|
||||
PX4IO_interface *interface;
|
||||
device::Device *interface;
|
||||
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
interface = io_serial_interface();
|
||||
interface = PX4IO_serial_interface();
|
||||
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
|
||||
interface = PX4IO_i2c_interface();
|
||||
#else
|
||||
# error Unknown board - cannot select interface.
|
||||
#endif
|
||||
|
|
@ -1615,7 +1617,7 @@ start(int argc, char *argv[])
|
|||
if (interface == nullptr)
|
||||
errx(1, "cannot alloc interface");
|
||||
|
||||
if (!interface->ok()) {
|
||||
if (interface->probe()) {
|
||||
delete interface;
|
||||
errx(1, "interface init failed");
|
||||
}
|
||||
|
|
@ -1739,12 +1741,12 @@ monitor(void)
|
|||
void
|
||||
if_test(unsigned mode)
|
||||
{
|
||||
PX4IO_interface *interface;
|
||||
device::Device *interface;
|
||||
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
interface = io_serial_interface();
|
||||
interface = PX4IO_serial_interface();
|
||||
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
|
||||
interface = PX4IO_i2c_interface();
|
||||
#else
|
||||
# error Unknown board - cannot select interface.
|
||||
#endif
|
||||
|
|
@ -1752,20 +1754,17 @@ if_test(unsigned mode)
|
|||
if (interface == nullptr)
|
||||
errx(1, "cannot alloc interface");
|
||||
|
||||
if (!interface->ok()) {
|
||||
if (interface->probe()) {
|
||||
delete interface;
|
||||
errx(1, "interface init failed");
|
||||
} else {
|
||||
|
||||
int result = interface->test(mode);
|
||||
int result = interface->ioctl(1, mode); /* XXX magic numbers */
|
||||
delete interface;
|
||||
errx(0, "test returned %d", result);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
}
|
||||
} /* namespace */
|
||||
|
||||
int
|
||||
px4io_main(int argc, char *argv[])
|
||||
|
|
|
|||
|
|
@ -52,109 +52,93 @@
|
|||
|
||||
#include <nuttx/i2c.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include "uploader.h"
|
||||
#include <modules/px4iofirmware/protocol.h>
|
||||
|
||||
#include "interface.h"
|
||||
device::Device *PX4IO_i2c_interface();
|
||||
|
||||
class PX4IO_I2C : public PX4IO_interface
|
||||
class PX4IO_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
PX4IO_I2C(int bus, uint8_t address);
|
||||
virtual ~PX4IO_I2C();
|
||||
|
||||
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
|
||||
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
|
||||
|
||||
virtual bool ok();
|
||||
|
||||
virtual int test(unsigned mode);
|
||||
virtual int probe();
|
||||
virtual int read(unsigned offset, void *data, unsigned count = 1);
|
||||
virtual int write(unsigned address, void *data, unsigned count = 1);
|
||||
virtual int ioctl(unsigned operation, unsigned &arg);
|
||||
|
||||
private:
|
||||
static const unsigned _retries = 2;
|
||||
|
||||
struct i2c_dev_s *_dev;
|
||||
uint8_t _address;
|
||||
};
|
||||
|
||||
PX4IO_interface *io_i2c_interface(int bus, uint8_t address)
|
||||
device::Device
|
||||
*PX4IO_i2c_interface()
|
||||
{
|
||||
return new PX4IO_I2C(bus, address);
|
||||
#ifdef PX4_I2C_OBDEV_PX4IO
|
||||
return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
|
||||
#endif
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
|
||||
_dev(nullptr),
|
||||
_address(address)
|
||||
I2C("PX4IO_i2c", nullptr, bus, address, 400000)
|
||||
{
|
||||
_dev = up_i2cinitialize(bus);
|
||||
if (_dev)
|
||||
I2C_SETFREQUENCY(_dev, 400000);
|
||||
_retries = 3;
|
||||
}
|
||||
|
||||
PX4IO_I2C::~PX4IO_I2C()
|
||||
{
|
||||
if (_dev)
|
||||
up_i2cuninitialize(_dev);
|
||||
}
|
||||
|
||||
bool
|
||||
PX4IO_I2C::ok()
|
||||
{
|
||||
if (!_dev)
|
||||
return false;
|
||||
|
||||
/* check any other status here */
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_I2C::test(unsigned mode)
|
||||
PX4IO_I2C::probe()
|
||||
{
|
||||
/* XXX really should do something here */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_I2C::ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
PX4IO_I2C::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
int ret;
|
||||
uint8_t page = address >> 8;
|
||||
uint8_t offset = address & 0xff;
|
||||
const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
|
||||
|
||||
/* set up the transfer */
|
||||
uint8_t addr[2] = {
|
||||
page,
|
||||
offset
|
||||
};
|
||||
|
||||
i2c_msg_s msgv[2];
|
||||
|
||||
msgv[0].addr = _address;
|
||||
msgv[0].flags = 0;
|
||||
msgv[0].buffer = addr;
|
||||
msgv[0].length = 2;
|
||||
|
||||
msgv[1].addr = _address;
|
||||
msgv[1].flags = I2C_M_NORESTART;
|
||||
msgv[1].buffer = (uint8_t *)values;
|
||||
msgv[1].length = num_values * sizeof(*values);
|
||||
msgv[1].length = 2 * count;
|
||||
|
||||
unsigned tries = 0;
|
||||
do {
|
||||
|
||||
/* perform the transfer */
|
||||
ret = I2C_TRANSFER(_dev, msgv, 2);
|
||||
|
||||
if (ret == OK)
|
||||
break;
|
||||
|
||||
} while (tries++ < _retries);
|
||||
|
||||
return ret;
|
||||
return transfer(msgv, 2);
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
|
||||
PX4IO_I2C::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
int ret;
|
||||
uint8_t page = address >> 8;
|
||||
uint8_t offset = address & 0xff;
|
||||
const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
|
||||
|
||||
/* set up the transfer */
|
||||
uint8_t addr[2] = {
|
||||
|
|
@ -163,25 +147,13 @@ PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_
|
|||
};
|
||||
i2c_msg_s msgv[2];
|
||||
|
||||
msgv[0].addr = _address;
|
||||
msgv[0].flags = 0;
|
||||
msgv[0].buffer = addr;
|
||||
msgv[0].length = 2;
|
||||
|
||||
msgv[1].addr = _address;
|
||||
msgv[1].flags = I2C_M_READ;
|
||||
msgv[1].buffer = (uint8_t *)values;
|
||||
msgv[1].length = num_values * sizeof(*values);
|
||||
msgv[1].length = 2 * count;
|
||||
|
||||
unsigned tries = 0;
|
||||
do {
|
||||
/* perform the transfer */
|
||||
ret = I2C_TRANSFER(_dev, msgv, 2);
|
||||
|
||||
if (ret == OK)
|
||||
break;
|
||||
|
||||
} while (tries++ < _retries);
|
||||
|
||||
return ret;
|
||||
return transfer(msgv, 2);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@
|
|||
|
||||
#include <debug.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/boards/px4fmuv2/px4fmu_internal.h> /* XXX should really not be hardcoding v2 here */
|
||||
|
||||
|
|
@ -66,7 +67,7 @@
|
|||
|
||||
#include <modules/px4iofirmware/protocol.h>
|
||||
|
||||
#include "interface.h"
|
||||
device::Device *PX4IO_serial_interface();
|
||||
|
||||
/* serial register accessors */
|
||||
#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
|
||||
|
|
@ -78,17 +79,16 @@
|
|||
#define rCR3 REG(STM32_USART_CR3_OFFSET)
|
||||
#define rGTPR REG(STM32_USART_GTPR_OFFSET)
|
||||
|
||||
class PX4IO_serial : public PX4IO_interface
|
||||
class PX4IO_serial : public device::Device
|
||||
{
|
||||
public:
|
||||
PX4IO_serial();
|
||||
virtual ~PX4IO_serial();
|
||||
|
||||
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
|
||||
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
|
||||
|
||||
virtual bool ok();
|
||||
virtual int test(unsigned mode);
|
||||
virtual int probe();
|
||||
virtual int read(unsigned offset, void *data, unsigned count = 1);
|
||||
virtual int write(unsigned address, void *data, unsigned count = 1);
|
||||
virtual int ioctl(unsigned operation, unsigned &arg);
|
||||
|
||||
private:
|
||||
/*
|
||||
|
|
@ -159,12 +159,14 @@ private:
|
|||
IOPacket PX4IO_serial::_dma_buffer;
|
||||
static PX4IO_serial *g_interface;
|
||||
|
||||
PX4IO_interface *io_serial_interface()
|
||||
device::Device
|
||||
*PX4IO_serial_interface()
|
||||
{
|
||||
return new PX4IO_serial();
|
||||
}
|
||||
|
||||
PX4IO_serial::PX4IO_serial() :
|
||||
Device("PX4IO_serial"),
|
||||
_tx_dma(nullptr),
|
||||
_rx_dma(nullptr),
|
||||
_rx_dma_status(_dma_status_inactive),
|
||||
|
|
@ -262,74 +264,87 @@ PX4IO_serial::~PX4IO_serial()
|
|||
g_interface = nullptr;
|
||||
}
|
||||
|
||||
bool
|
||||
PX4IO_serial::ok()
|
||||
int
|
||||
PX4IO_serial::probe()
|
||||
{
|
||||
if (_tx_dma == nullptr)
|
||||
return false;
|
||||
if (_rx_dma == nullptr)
|
||||
return false;
|
||||
/* XXX this could try talking to IO */
|
||||
|
||||
return true;
|
||||
if (_tx_dma == nullptr)
|
||||
return -1;
|
||||
if (_rx_dma == nullptr)
|
||||
return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_serial::test(unsigned mode)
|
||||
PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
|
||||
{
|
||||
|
||||
switch (mode) {
|
||||
case 0:
|
||||
lowsyslog("test 0\n");
|
||||
switch (operation) {
|
||||
|
||||
/* kill DMA, this is a PIO test */
|
||||
stm32_dmastop(_tx_dma);
|
||||
stm32_dmastop(_rx_dma);
|
||||
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
|
||||
case 1: /* XXX magic number - test operation */
|
||||
switch (arg) {
|
||||
case 0:
|
||||
lowsyslog("test 0\n");
|
||||
|
||||
for (;;) {
|
||||
while (!(rSR & USART_SR_TXE))
|
||||
;
|
||||
rDR = 0x55;
|
||||
}
|
||||
return 0;
|
||||
/* kill DMA, this is a PIO test */
|
||||
stm32_dmastop(_tx_dma);
|
||||
stm32_dmastop(_rx_dma);
|
||||
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
|
||||
|
||||
case 1:
|
||||
{
|
||||
unsigned fails = 0;
|
||||
for (unsigned count = 0;; count++) {
|
||||
uint16_t value = count & 0xffff;
|
||||
|
||||
if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0)
|
||||
fails++;
|
||||
|
||||
if (count >= 5000) {
|
||||
lowsyslog("==== test 1 : %u failures ====\n", fails);
|
||||
perf_print_counter(_pc_txns);
|
||||
perf_print_counter(_pc_dmasetup);
|
||||
perf_print_counter(_pc_retries);
|
||||
perf_print_counter(_pc_timeouts);
|
||||
perf_print_counter(_pc_crcerrs);
|
||||
perf_print_counter(_pc_dmaerrs);
|
||||
perf_print_counter(_pc_protoerrs);
|
||||
perf_print_counter(_pc_uerrs);
|
||||
perf_print_counter(_pc_idle);
|
||||
perf_print_counter(_pc_badidle);
|
||||
count = 0;
|
||||
}
|
||||
for (;;) {
|
||||
while (!(rSR & USART_SR_TXE))
|
||||
;
|
||||
rDR = 0x55;
|
||||
}
|
||||
return 0;
|
||||
|
||||
case 1:
|
||||
{
|
||||
unsigned fails = 0;
|
||||
for (unsigned count = 0;; count++) {
|
||||
uint16_t value = count & 0xffff;
|
||||
|
||||
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
|
||||
fails++;
|
||||
|
||||
if (count >= 5000) {
|
||||
lowsyslog("==== test 1 : %u failures ====\n", fails);
|
||||
perf_print_counter(_pc_txns);
|
||||
perf_print_counter(_pc_dmasetup);
|
||||
perf_print_counter(_pc_retries);
|
||||
perf_print_counter(_pc_timeouts);
|
||||
perf_print_counter(_pc_crcerrs);
|
||||
perf_print_counter(_pc_dmaerrs);
|
||||
perf_print_counter(_pc_protoerrs);
|
||||
perf_print_counter(_pc_uerrs);
|
||||
perf_print_counter(_pc_idle);
|
||||
perf_print_counter(_pc_badidle);
|
||||
count = 0;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
case 2:
|
||||
lowsyslog("test 2\n");
|
||||
return 0;
|
||||
}
|
||||
case 2:
|
||||
lowsyslog("test 2\n");
|
||||
return 0;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
PX4IO_serial::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
if (num_values > PKT_MAX_REGS) {
|
||||
uint8_t page = address >> 8;
|
||||
uint8_t offset = address & 0xff;
|
||||
const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
|
||||
|
||||
if (count > PKT_MAX_REGS) {
|
||||
errno = EINVAL;
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -339,11 +354,11 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi
|
|||
int result;
|
||||
for (unsigned retries = 0; retries < 3; retries++) {
|
||||
|
||||
_dma_buffer.count_code = num_values | PKT_CODE_WRITE;
|
||||
_dma_buffer.count_code = count | PKT_CODE_WRITE;
|
||||
_dma_buffer.page = page;
|
||||
_dma_buffer.offset = offset;
|
||||
memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values));
|
||||
for (unsigned i = num_values; i < PKT_MAX_REGS; i++)
|
||||
memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
|
||||
for (unsigned i = count; i < PKT_MAX_REGS; i++)
|
||||
_dma_buffer.regs[i] = 0x55aa;
|
||||
|
||||
/* XXX implement check byte */
|
||||
|
|
@ -373,9 +388,13 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi
|
|||
}
|
||||
|
||||
int
|
||||
PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
|
||||
PX4IO_serial::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
if (num_values > PKT_MAX_REGS)
|
||||
uint8_t page = address >> 8;
|
||||
uint8_t offset = address & 0xff;
|
||||
uint16_t *values = reinterpret_cast<uint16_t *>(data);
|
||||
|
||||
if (count > PKT_MAX_REGS)
|
||||
return -EINVAL;
|
||||
|
||||
sem_wait(&_bus_semaphore);
|
||||
|
|
@ -383,7 +402,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
|
|||
int result;
|
||||
for (unsigned retries = 0; retries < 3; retries++) {
|
||||
|
||||
_dma_buffer.count_code = num_values | PKT_CODE_READ;
|
||||
_dma_buffer.count_code = count | PKT_CODE_READ;
|
||||
_dma_buffer.page = page;
|
||||
_dma_buffer.offset = offset;
|
||||
|
||||
|
|
@ -402,7 +421,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
|
|||
perf_count(_pc_protoerrs);
|
||||
|
||||
/* compare the received count with the expected count */
|
||||
} else if (PKT_COUNT(_dma_buffer) != num_values) {
|
||||
} else if (PKT_COUNT(_dma_buffer) != count) {
|
||||
|
||||
/* IO returned the wrong number of registers - no point retrying */
|
||||
errno = EIO;
|
||||
|
|
@ -413,14 +432,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
|
|||
} else {
|
||||
|
||||
/* copy back the result */
|
||||
memcpy(values, &_dma_buffer.regs[0], (2 * num_values));
|
||||
memcpy(values, &_dma_buffer.regs[0], (2 * count));
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
perf_count(_pc_retries);
|
||||
}
|
||||
out:
|
||||
|
||||
sem_post(&_bus_semaphore);
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue