mirror of https://github.com/ArduPilot/ardupilot
parent
ed79e8c954
commit
9373a4e5b3
|
@ -5,6 +5,7 @@
|
|||
namespace PX4 {
|
||||
class PX4ConsoleDriver;
|
||||
class PX4Scheduler;
|
||||
class PX4UARTDriver;
|
||||
}
|
||||
|
||||
#endif //__AP_HAL_PX4_NAMESPACE_H__
|
||||
|
|
|
@ -3,14 +3,16 @@
|
|||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
#include <stdarg.h>
|
||||
#include "Console.h"
|
||||
#include "UARTDriver.h"
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace PX4;
|
||||
|
||||
PX4ConsoleDriver::PX4ConsoleDriver() {}
|
||||
|
||||
void PX4ConsoleDriver::init(void* unused)
|
||||
void PX4ConsoleDriver::init(void* uart)
|
||||
{
|
||||
_uart = (PX4UARTDriver *)uart;
|
||||
}
|
||||
|
||||
void PX4ConsoleDriver::backend_open()
|
||||
|
@ -38,44 +40,43 @@ void PX4ConsoleDriver::println_P(const prog_char_t *pstr) {
|
|||
void PX4ConsoleDriver::printf(const char *fmt, ...) {
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vfprintf(stdout, fmt, ap);
|
||||
_uart->vprintf(fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
void PX4ConsoleDriver::_printf_P(const prog_char *fmt, ...) {
|
||||
va_list ap;
|
||||
va_start(ap,fmt);
|
||||
vfprintf(stdout, fmt, ap);
|
||||
va_start(ap, fmt);
|
||||
_uart->vprintf(fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
void PX4ConsoleDriver::vprintf(const char *fmt, va_list ap) {
|
||||
vfprintf(stdout, fmt, ap);
|
||||
_uart->vprintf(fmt, ap);
|
||||
}
|
||||
|
||||
void PX4ConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap) {
|
||||
vfprintf(stdout, fmt, ap);
|
||||
_uart->vprintf(fmt, ap);
|
||||
}
|
||||
|
||||
int16_t PX4ConsoleDriver::available() {
|
||||
return 0;
|
||||
return _uart->available();
|
||||
}
|
||||
|
||||
int16_t PX4ConsoleDriver::txspace() {
|
||||
return 0;
|
||||
return _uart->txspace();
|
||||
}
|
||||
|
||||
int16_t PX4ConsoleDriver::read() {
|
||||
return 0;
|
||||
return _uart->read();
|
||||
}
|
||||
|
||||
int16_t PX4ConsoleDriver::peek() {
|
||||
return 0;
|
||||
return _uart->peek();
|
||||
}
|
||||
|
||||
size_t PX4ConsoleDriver::write(uint8_t c) {
|
||||
fputc(c, stdout);
|
||||
return 1;
|
||||
return _uart->write(c);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -27,6 +27,9 @@ public:
|
|||
int16_t peek();
|
||||
|
||||
size_t write(uint8_t c);
|
||||
|
||||
private:
|
||||
PX4UARTDriver *_uart;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_PX4_CONSOLE_H__
|
||||
|
|
|
@ -9,6 +9,7 @@
|
|||
#include "HAL_PX4_Class.h"
|
||||
#include "Console.h"
|
||||
#include "Scheduler.h"
|
||||
#include "UARTDriver.h"
|
||||
|
||||
#include <AP_HAL_Empty.h>
|
||||
#include <AP_HAL_Empty_Private.h>
|
||||
|
@ -17,9 +18,6 @@
|
|||
|
||||
using namespace PX4;
|
||||
|
||||
static Empty::EmptyUARTDriver uartADriver;
|
||||
static Empty::EmptyUARTDriver uartBDriver;
|
||||
static Empty::EmptyUARTDriver uartCDriver;
|
||||
static Empty::EmptyI2CDriver i2cDriver;
|
||||
static Empty::EmptySPIDeviceManager spiDeviceManager;
|
||||
static Empty::EmptyAnalogIn analogIn;
|
||||
|
@ -32,6 +30,11 @@ static Empty::EmptyUtil utilInstance;
|
|||
static PX4ConsoleDriver consoleDriver;
|
||||
static PX4Scheduler schedulerInstance;
|
||||
|
||||
// only one real UART driver for now
|
||||
static PX4UARTDriver uartADriver;
|
||||
static Empty::EmptyUARTDriver uartBDriver;
|
||||
static Empty::EmptyUARTDriver uartCDriver;
|
||||
|
||||
HAL_PX4::HAL_PX4() :
|
||||
AP_HAL::HAL(
|
||||
&uartADriver, /* uartA */
|
||||
|
|
|
@ -0,0 +1,78 @@
|
|||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
#include "UARTDriver.h"
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
|
||||
using namespace PX4;
|
||||
|
||||
PX4UARTDriver::PX4UARTDriver() {}
|
||||
|
||||
/*
|
||||
this UART driver just maps to stdin/stdout, which goes to
|
||||
whatever is setup for the PX4 console. Baud rate control is not
|
||||
available.
|
||||
*/
|
||||
|
||||
void PX4UARTDriver::begin(uint32_t b) {}
|
||||
void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) {}
|
||||
void PX4UARTDriver::end() {}
|
||||
void PX4UARTDriver::flush() {}
|
||||
bool PX4UARTDriver::is_initialized() { return true; }
|
||||
void PX4UARTDriver::set_blocking_writes(bool blocking) {}
|
||||
bool PX4UARTDriver::tx_pending() { return false; }
|
||||
|
||||
/* PX4 implementations of BetterStream virtual methods */
|
||||
void PX4UARTDriver::print_P(const prog_char_t *pstr) {
|
||||
print(pstr);
|
||||
}
|
||||
|
||||
void PX4UARTDriver::println_P(const prog_char_t *pstr) {
|
||||
println(pstr);
|
||||
}
|
||||
|
||||
void PX4UARTDriver::printf(const char *fmt, ...) {
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vfprintf(stdout, fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vfprintf(stdout, fmt, ap);
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
|
||||
vfprintf(stdout, fmt, ap);
|
||||
}
|
||||
|
||||
void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
|
||||
vfprintf(stdout, fmt, ap);
|
||||
}
|
||||
|
||||
/* PX4 implementations of Stream virtual methods */
|
||||
int16_t PX4UARTDriver::available() { return 0; }
|
||||
int16_t PX4UARTDriver::txspace() { return 128; }
|
||||
|
||||
int16_t PX4UARTDriver::read() {
|
||||
uint8_t c;
|
||||
if (fread(&c, 1, 1, stdin) == 1) {
|
||||
return c;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int16_t PX4UARTDriver::peek() {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* PX4 implementations of Print virtual methods */
|
||||
size_t PX4UARTDriver::write(uint8_t c) {
|
||||
return fwrite(&c, 1, 1, stdout);
|
||||
}
|
||||
|
||||
#endif
|
|
@ -0,0 +1,38 @@
|
|||
|
||||
#ifndef __AP_HAL_PX4_UARTDRIVER_H__
|
||||
#define __AP_HAL_PX4_UARTDRIVER_H__
|
||||
|
||||
#include <AP_HAL_PX4.h>
|
||||
|
||||
class PX4::PX4UARTDriver : public AP_HAL::UARTDriver {
|
||||
public:
|
||||
PX4UARTDriver();
|
||||
/* PX4 implementations of UARTDriver virtual methods */
|
||||
void begin(uint32_t b);
|
||||
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
|
||||
void end();
|
||||
void flush();
|
||||
bool is_initialized();
|
||||
void set_blocking_writes(bool blocking);
|
||||
bool tx_pending();
|
||||
|
||||
/* PX4 implementations of BetterStream virtual methods */
|
||||
void print_P(const prog_char_t *pstr);
|
||||
void println_P(const prog_char_t *pstr);
|
||||
void printf(const char *pstr, ...);
|
||||
void _printf_P(const prog_char *pstr, ...);
|
||||
|
||||
void vprintf(const char* fmt, va_list ap);
|
||||
void vprintf_P(const prog_char* fmt, va_list ap);
|
||||
|
||||
/* PX4 implementations of Stream virtual methods */
|
||||
int16_t available();
|
||||
int16_t txspace();
|
||||
int16_t read();
|
||||
int16_t peek();
|
||||
|
||||
/* PX4 implementations of Print virtual methods */
|
||||
size_t write(uint8_t c);
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_PX4_UARTDRIVER_H__
|
Loading…
Reference in New Issue