fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 16:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Build the px4io interfaces on top of the Device direct-access API.

Browse Source
This commit is contained in:
px4dev
2013-07-13 21:00:27 -07:00
parent 26eb0b9d72
commit 4f0ae1cdea
3 changed files with 140 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+18 -19
View File
@@ -85,7 +85,9 @@
#include <modules/px4iofirmware/protocol.h> #include <modules/px4iofirmware/protocol.h>
#include "uploader.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_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
@@ -93,7 +95,7 @@
class PX4IO : public device::CDev class PX4IO : public device::CDev
{ {
public: public:
PX4IO(PX4IO_interface *interface); PX4IO(device::Device *interface);
virtual ~PX4IO(); virtual ~PX4IO();
virtual int init(); virtual int init();
@@ -131,7 +133,7 @@ public:
void print_status(); void print_status();
private: private:
PX4IO_interface *_interface; device::Device *_interface;
// XXX // XXX
unsigned _hardware; unsigned _hardware;
@@ -316,7 +318,7 @@ PX4IO *g_dev;
} }
PX4IO::PX4IO(PX4IO_interface *interface) : PX4IO::PX4IO(device::Device *interface) :
CDev("px4io", "/dev/px4io"), CDev("px4io", "/dev/px4io"),
_interface(interface), _interface(interface),
_hardware(0), _hardware(0),
@@ -1129,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
return -EINVAL; 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) if (ret != OK)
debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno);
return ret; return ret;
@@ -1150,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
return -EINVAL; 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) if (ret != OK)
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno);
return ret; return ret;
@@ -1602,12 +1604,12 @@ start(int argc, char *argv[])
if (g_dev != nullptr) if (g_dev != nullptr)
errx(1, "already loaded"); errx(1, "already loaded");
PX4IO_interface *interface; device::Device *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
interface = io_serial_interface(); interface = PX4IO_serial_interface();
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); interface = PX4IO_i2c_interface();
#else #else
# error Unknown board - cannot select interface. # error Unknown board - cannot select interface.
#endif #endif
@@ -1615,7 +1617,7 @@ start(int argc, char *argv[])
if (interface == nullptr) if (interface == nullptr)
errx(1, "cannot alloc interface"); errx(1, "cannot alloc interface");
if (!interface->ok()) { if (interface->probe()) {
delete interface; delete interface;
errx(1, "interface init failed"); errx(1, "interface init failed");
} }
@@ -1739,12 +1741,12 @@ monitor(void)
void void
if_test(unsigned mode) if_test(unsigned mode)
{ {
PX4IO_interface *interface; device::Device *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
interface = io_serial_interface(); interface = PX4IO_serial_interface();
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); interface = PX4IO_i2c_interface();
#else #else
# error Unknown board - cannot select interface. # error Unknown board - cannot select interface.
#endif #endif
@@ -1752,20 +1754,17 @@ if_test(unsigned mode)
if (interface == nullptr) if (interface == nullptr)
errx(1, "cannot alloc interface"); errx(1, "cannot alloc interface");
if (!interface->ok()) { if (interface->probe()) {
delete interface; delete interface;
errx(1, "interface init failed"); errx(1, "interface init failed");
} else { } else {
int result = interface->ioctl(1, mode); /* XXX magic numbers */
int result = interface->test(mode);
delete interface; delete interface;
errx(0, "test returned %d", result); errx(0, "test returned %d", result);
} }
exit(0);
} }
} } /* namespace */
int int
px4io_main(int argc, char *argv[]) px4io_main(int argc, char *argv[])
src/drivers/px4io/px4io_i2c.cpp
+37 -65
View File
@@ -52,109 +52,93 @@
#include <nuttx/i2c.h> #include <nuttx/i2c.h>
#include <drivers/device/i2c.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include "uploader.h" #include "uploader.h"
#include <modules/px4iofirmware/protocol.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: public:
PX4IO_I2C(int bus, uint8_t address); PX4IO_I2C(int bus, uint8_t address);
virtual ~PX4IO_I2C(); virtual ~PX4IO_I2C();
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); virtual int probe();
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); virtual int read(unsigned offset, void *data, unsigned count = 1);
virtual int write(unsigned address, void *data, unsigned count = 1);
virtual bool ok(); virtual int ioctl(unsigned operation, unsigned &arg);
virtual int test(unsigned mode);
private: 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) : PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
_dev(nullptr), I2C("PX4IO_i2c", nullptr, bus, address, 400000)
_address(address)
{ {
_dev = up_i2cinitialize(bus); _retries = 3;
if (_dev)
I2C_SETFREQUENCY(_dev, 400000);
} }
PX4IO_I2C::~PX4IO_I2C() 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 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; return 0;
} }
int 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 */ /* set up the transfer */
uint8_t addr[2] = { uint8_t addr[2] = {
page, page,
offset offset
}; };
i2c_msg_s msgv[2]; i2c_msg_s msgv[2];
msgv[0].addr = _address;
msgv[0].flags = 0; msgv[0].flags = 0;
msgv[0].buffer = addr; msgv[0].buffer = addr;
msgv[0].length = 2; msgv[0].length = 2;
msgv[1].addr = _address;
msgv[1].flags = I2C_M_NORESTART; msgv[1].flags = I2C_M_NORESTART;
msgv[1].buffer = (uint8_t *)values; msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values); msgv[1].length = 2 * count;
unsigned tries = 0; return transfer(msgv, 2);
do {
/* perform the transfer */
ret = I2C_TRANSFER(_dev, msgv, 2);
if (ret == OK)
break;
} while (tries++ < _retries);
return ret;
} }
int 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 */ /* set up the transfer */
uint8_t addr[2] = { 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]; i2c_msg_s msgv[2];
msgv[0].addr = _address;
msgv[0].flags = 0; msgv[0].flags = 0;
msgv[0].buffer = addr; msgv[0].buffer = addr;
msgv[0].length = 2; msgv[0].length = 2;
msgv[1].addr = _address;
msgv[1].flags = I2C_M_READ; msgv[1].flags = I2C_M_READ;
msgv[1].buffer = (uint8_t *)values; msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values); msgv[1].length = 2 * count;
unsigned tries = 0; return transfer(msgv, 2);
do {
/* perform the transfer */
ret = I2C_TRANSFER(_dev, msgv, 2);
if (ret == OK)
break;
} while (tries++ < _retries);
return ret;
} }
src/drivers/px4io/px4io_serial.cpp
+85 -66
View File
@@ -59,6 +59,7 @@
#include <debug.h> #include <debug.h>
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/boards/px4fmuv2/px4fmu_internal.h> /* XXX should really not be hardcoding v2 here */ #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 <modules/px4iofirmware/protocol.h>
#include "interface.h" device::Device *PX4IO_serial_interface();
/* serial register accessors */ /* serial register accessors */
#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
@@ -78,17 +79,16 @@
#define rCR3 REG(STM32_USART_CR3_OFFSET) #define rCR3 REG(STM32_USART_CR3_OFFSET)
#define rGTPR REG(STM32_USART_GTPR_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET)
class PX4IO_serial : public PX4IO_interface class PX4IO_serial : public device::Device
{ {
public: public:
PX4IO_serial(); PX4IO_serial();
virtual ~PX4IO_serial(); virtual ~PX4IO_serial();
virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); virtual int probe();
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); virtual int read(unsigned offset, void *data, unsigned count = 1);
virtual int write(unsigned address, void *data, unsigned count = 1);
virtual bool ok(); virtual int ioctl(unsigned operation, unsigned &arg);
virtual int test(unsigned mode);
private: private:
/* /*
@@ -159,12 +159,14 @@ private:
IOPacket PX4IO_serial::_dma_buffer; IOPacket PX4IO_serial::_dma_buffer;
static PX4IO_serial *g_interface; static PX4IO_serial *g_interface;
PX4IO_interface *io_serial_interface() device::Device
*PX4IO_serial_interface()
{ {
return new PX4IO_serial(); return new PX4IO_serial();
} }
PX4IO_serial::PX4IO_serial() : PX4IO_serial::PX4IO_serial() :
Device("PX4IO_serial"),
_tx_dma(nullptr), _tx_dma(nullptr),
_rx_dma(nullptr), _rx_dma(nullptr),
_rx_dma_status(_dma_status_inactive), _rx_dma_status(_dma_status_inactive),
@@ -262,74 +264,87 @@ PX4IO_serial::~PX4IO_serial()
g_interface = nullptr; g_interface = nullptr;
} }
bool int
PX4IO_serial::ok() PX4IO_serial::probe()
{ {
if (_tx_dma == nullptr) /* XXX this could try talking to IO */
return false;
if (_rx_dma == nullptr)
return false;
return true; if (_tx_dma == nullptr)
return -1;
if (_rx_dma == nullptr)
return -1;
return 0;
} }
int int
PX4IO_serial::test(unsigned mode) PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
{ {
switch (mode) { switch (operation) {
case 0:
lowsyslog("test 0\n");
/* kill DMA, this is a PIO test */ case 1: /* XXX magic number - test operation */
stm32_dmastop(_tx_dma); switch (arg) {
stm32_dmastop(_rx_dma); case 0:
rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); lowsyslog("test 0\n");
for (;;) { /* kill DMA, this is a PIO test */
while (!(rSR & USART_SR_TXE)) stm32_dmastop(_tx_dma);
; stm32_dmastop(_rx_dma);
rDR = 0x55; rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
}
return 0;
case 1: for (;;) {
{ while (!(rSR & USART_SR_TXE))
unsigned fails = 0; ;
for (unsigned count = 0;; count++) { rDR = 0x55;
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;
}
} }
return 0; 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: default:
lowsyslog("test 2\n"); break;
return 0;
} }
return -1; return -1;
} }
int 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; errno = EINVAL;
return -1; return -1;
} }
@@ -339,11 +354,11 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi
int result; int result;
for (unsigned retries = 0; retries < 3; retries++) { 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.page = page;
_dma_buffer.offset = offset; _dma_buffer.offset = offset;
memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
for (unsigned i = num_values; i < PKT_MAX_REGS; i++) for (unsigned i = count; i < PKT_MAX_REGS; i++)
_dma_buffer.regs[i] = 0x55aa; _dma_buffer.regs[i] = 0x55aa;
/* XXX implement check byte */ /* XXX implement check byte */
@@ -373,9 +388,13 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi
} }
int 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; return -EINVAL;
sem_wait(&_bus_semaphore); 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; int result;
for (unsigned retries = 0; retries < 3; retries++) { 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.page = page;
_dma_buffer.offset = offset; _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); perf_count(_pc_protoerrs);
/* compare the received count with the expected count */ /* 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 */ /* IO returned the wrong number of registers - no point retrying */
errno = EIO; errno = EIO;
@@ -413,14 +432,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
} else { } else {
/* copy back the result */ /* copy back the result */
memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); memcpy(values, &_dma_buffer.regs[0], (2 * count));
} }
break; break;
} }
perf_count(_pc_retries); perf_count(_pc_retries);
} }
out:
sem_post(&_bus_semaphore); sem_post(&_bus_semaphore);
return result; return result;
} }