fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add a test hook for the PX4IO interface. Wire up some simple tests for the serial interface.

Browse Source 
Signal quality looks good at 1.5Mbps. Transmit timing is ~450µs per packet, ~20µs packet-to-packet delay spinning in a loop transmitting.
This commit is contained in:
px4dev
2013-07-03 23:41:40 -07:00
parent 03a15bfdc5
commit 7255c02c20
4 changed files with 225 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/interface.h
+8 -1
View File
@@ -72,7 +72,14 @@ public:
* @return Zero on success.
*/
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0;
/**
* Perform an interface test.
*
* @param mode Optional test mode.
*/
virtual int test(unsigned mode) = 0;
};
extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address);
extern PX4IO_interface *io_serial_interface(int port);
extern PX4IO_interface *io_serial_interface();
src/drivers/px4io/interface_i2c.cpp
+8
View File
@@ -69,6 +69,8 @@ public:
virtual bool ok();
virtual int test(unsigned mode);
private:
static const unsigned _retries = 2;
@@ -107,6 +109,12 @@ PX4IO_I2C::ok()
return true;
}
int
PX4IO_I2C::test(unsigned mode)
{
return 0;
}
int
PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
src/drivers/px4io/interface_serial.cpp
+59 -10
View File
@@ -87,6 +87,7 @@ public:
virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
virtual bool ok();
virtual int test(unsigned mode);
private:
/*
@@ -102,10 +103,7 @@ private:
*/
static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory
static const unsigned _serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP;
DMA_HANDLE _tx_dma;
static const unsigned _serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP;
DMA_HANDLE _rx_dma;
/** set if we have started a transaction that expects a reply */
@@ -164,7 +162,7 @@ private:
IOPacket PX4IO_serial::_dma_buffer;
PX4IO_interface *io_serial_interface(int port)
PX4IO_interface *io_serial_interface()
{
return new PX4IO_serial();
}
@@ -175,8 +173,8 @@ PX4IO_serial::PX4IO_serial() :
_expect_reply(false)
{
/* allocate DMA */
_tx_dma = stm32_dmachannel(_serial_tx_dma);
_rx_dma = stm32_dmachannel(_serial_rx_dma);
_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
return;
@@ -209,14 +207,25 @@ PX4IO_serial::PX4IO_serial() :
PX4IO_serial::~PX4IO_serial()
{
if (_tx_dma != nullptr)
if (_tx_dma != nullptr) {
stm32_dmastop(_tx_dma);
stm32_dmafree(_tx_dma);
if (_rx_dma != nullptr)
}
if (_rx_dma != nullptr) {
stm32_dmastop(_rx_dma);
stm32_dmafree(_rx_dma);
}
/* reset the UART */
_CR1(0);
_CR2(0);
_CR3(0);
/* restore the GPIOs */
stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
/* and kill our semaphores */
sem_destroy(&_completion_semaphore);
sem_destroy(&_bus_semaphore);
}
@@ -232,6 +241,40 @@ PX4IO_serial::ok()
return true;
}
int
PX4IO_serial::test(unsigned mode)
{
switch (mode) {
case 0:
lowsyslog("test 0\n");
/* kill DMA, this is a PIO test */
stm32_dmastop(_tx_dma);
stm32_dmastop(_rx_dma);
_CR3(_CR3() & ~(USART_CR3_DMAR | USART_CR3_DMAT));
for (;;) {
while (!(_SR() & USART_SR_TXE))
;
_DR(0x55);
}
return 0;
case 1:
lowsyslog("test 1\n");
{
uint16_t value = 0x5555;
for (;;) {
if (set_reg(0x55, 0x55, &value, 1) != OK)
return -2;
}
return 0;
}
}
return -1;
}
int
PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -297,16 +340,20 @@ PX4IO_serial::_wait_complete(bool expect_reply)
stm32_dmastart(_rx_dma, _dma_callback, this, false);
/* start TX DMA - no callback if we also expect a reply */
stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false);
stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false);
/* compute the deadline for a 5ms timeout */
struct timespec abstime;
clock_gettime(CLOCK_REALTIME, &abstime);
#if 1
abstime.tv_sec++;
#else
abstime.tv_nsec += 5000000; /* 5ms timeout */
while (abstime.tv_nsec > 1000000000) {
abstime.tv_sec++;
abstime.tv_nsec -= 1000000000;
}
#endif
/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
int ret;
@@ -316,11 +363,13 @@ PX4IO_serial::_wait_complete(bool expect_reply)
if (ret == OK)
break;
if (ret == ETIMEDOUT) {
if (errno == ETIMEDOUT) {
lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma));
/* something has broken - clear out any partial DMA state and reconfigure */
_reset_dma();
break;
}
lowsyslog("unexpected ret %d/%d\n", ret, errno);
}
return ret;
src/drivers/px4io/px4io.cpp
+150 -132
View File
@@ -104,8 +104,8 @@ public:
/**
* Set the update rate for actuator outputs from FMU to IO.
*
* @param rate The rate in Hz actuator outpus are sent to IO.
* Min 10 Hz, max 400 Hz
* @param rate The rate in Hz actuator outpus are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
@@ -120,14 +120,14 @@ public:
/**
* Push failsafe values to IO.
*
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param len Number of channels, could up to 8
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Print the current status of IO
*/
* Print the current status of IO
*/
void print_status();
private:
@@ -1579,7 +1579,7 @@ start(int argc, char *argv[])
PX4IO_interface *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
interface = io_serial_interface(5); /* XXX wrong port! */
interface = io_serial_interface();
#elif defined(CONFIG_ARCH_BOARD_PX4FMU)
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
#else
@@ -1710,6 +1710,35 @@ monitor(void)
}
}
void
if_test(unsigned mode)
{
PX4IO_interface *interface;
#if defined(CONFIG_ARCH_BOARD_PX4FMUV2)
interface = io_serial_interface();
#elif defined(CONFIG_ARCH_BOARD_PX4FMU)
interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO);
#else
# error Unknown board - cannot select interface.
#endif
if (interface == nullptr)
errx(1, "cannot alloc interface");
if (!interface->ok()) {
delete interface;
errx(1, "interface init failed");
} else {
int result = interface->test(mode);
delete interface;
errx(0, "test returned %d", result);
}
exit(0);
}
}
int
@@ -1722,130 +1751,6 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
if (!strcmp(argv[1], "limit")) {
if (g_dev != nullptr) {
if ((argc > 2)) {
g_dev->set_update_rate(atoi(argv[2]));
} else {
errx(1, "missing argument (50 - 200 Hz)");
return 1;
}
}
exit(0);
}
if (!strcmp(argv[1], "current")) {
if (g_dev != nullptr) {
if ((argc > 3)) {
g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
} else {
errx(1, "missing argument (apm_per_volt, amp_offset)");
return 1;
}
}
exit(0);
}
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < 800 || failsafe[i] > 2200) {
errx(1, "value out of range of 800 < value < 2200. Aborting.");
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
/*
* Enable in-air restart support.
* We can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (g_dev != nullptr) {
/* stop the driver */
delete g_dev;
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (g_dev != nullptr) {
printf("[px4io] loaded\n");
g_dev->print_status();
} else {
printf("[px4io] not loaded\n");
}
exit(0);
}
if (!strcmp(argv[1], "debug")) {
if (argc <= 2) {
printf("usage: px4io debug LEVEL\n");
exit(1);
}
if (g_dev == nullptr) {
printf("px4io is not started\n");
exit(1);
}
uint8_t level = atoi(argv[2]);
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
if (ret != 0) {
printf("SET_DEBUG failed - %d\n", ret);
exit(1);
}
printf("SET_DEBUG %u OK\n", (unsigned)level);
exit(0);
}
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
@@ -1896,6 +1801,119 @@ px4io_main(int argc, char *argv[])
return ret;
}
if (!strcmp(argv[1], "iftest")) {
if (g_dev != nullptr)
errx(1, "can't iftest when started");
if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0);
}
/* commands below here require a started driver */
if (g_dev == nullptr)
errx(1, "not started");
if (!strcmp(argv[1], "limit")) {
if ((argc > 2)) {
g_dev->set_update_rate(atoi(argv[2]));
} else {
errx(1, "missing argument (50 - 200 Hz)");
return 1;
}
exit(0);
}
if (!strcmp(argv[1], "current")) {
if ((argc > 3)) {
g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
} else {
errx(1, "missing argument (apm_per_volt, amp_offset)");
return 1;
}
exit(0);
}
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
/* set channel to commandline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < 800 || failsafe[i] > 2200) {
errx(1, "value out of range of 800 < value < 2200. Aborting.");
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
/*
* Enable in-air restart support.
* We can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
/* stop the driver */
delete g_dev;
exit(0);
}
if (!strcmp(argv[1], "status")) {
printf("[px4io] loaded\n");
g_dev->print_status();
exit(0);
}
if (!strcmp(argv[1], "debug")) {
if (argc <= 2) {
printf("usage: px4io debug LEVEL\n");
exit(1);
}
if (g_dev == nullptr) {
printf("px4io is not started\n");
exit(1);
}
uint8_t level = atoi(argv[2]);
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
if (ret != 0) {
printf("SET_DEBUG failed - %d\n", ret);
exit(1);
}
printf("SET_DEBUG %u OK\n", (unsigned)level);
exit(0);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
@@ -1909,6 +1927,6 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "monitor"))
monitor();
out:
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
}