AP_HAL: move in tap esc implementation

This can be shared with Linux.
This commit is contained in:
Lucas De Marchi 2017-05-18 17:24:02 -07:00
parent a9f257ddc6
commit f6b1099896
3 changed files with 15 additions and 18 deletions

View File

@ -54,6 +54,7 @@
#include "RCOutput_Tap.h"
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
@ -61,8 +62,6 @@
#include <termios.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <AP_Math/AP_Math.h>
#define DEBUG 0
@ -79,7 +78,6 @@ extern const AP_HAL::HAL &hal;
/****** ESC data types ******/
#define ESC_HAVE_CURRENT_SENSOR
#define MIN_BOOT_TIME_USEC (550 * AP_USEC_PER_MSEC)
static const uint8_t crcTable[256] = {
0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,
@ -137,7 +135,7 @@ static const uint8_t device_dir_map[] = {0, 1, 0, 1, 0, 1, 0, 1};
#define MIN_BOOT_TIME_MSEC (550) // Minimum time to wait after Power on before sending commands
namespace PX4 {
namespace ap {
/****** Run ***********/
@ -304,7 +302,7 @@ enum PARSR_ESC_STATE {
/****************************/
}
using namespace PX4;
using namespace ap;
void RCOutput_Tap::_uart_close()
{
@ -322,7 +320,7 @@ bool RCOutput_Tap::_uart_open()
int termios_state = -1;
if (_uart_fd < 0) {
PX4_ERR("failed to open uart device!");
::fprintf(stderr, "failed to open uart device! %s\n", UART_DEVICE_PATH);
return -1;
}
@ -353,16 +351,16 @@ bool RCOutput_Tap::_uart_open()
void RCOutput_Tap::init()
{
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
_perf_rcout = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "APM_rcout");
if (!_uart_open()) {
AP_HAL::panic("Unable to open " UART_DEVICE_PATH);
return;
}
hrt_abstime uptime_usec = hrt_absolute_time();
if (uptime_usec < MIN_BOOT_TIME_USEC) {
hal.scheduler->delay((MIN_BOOT_TIME_USEC - uptime_usec) / AP_USEC_PER_MSEC);
uint32_t now = AP_HAL::millis();
if (now < MIN_BOOT_TIME_MSEC) {
hal.scheduler->delay(MIN_BOOT_TIME_MSEC - now);
}
/* Issue Basic Config */
@ -504,7 +502,7 @@ void RCOutput_Tap::push()
{
_corking = false;
perf_begin(_perf_rcout);
hal.util->perf_begin(_perf_rcout);
uint16_t out[TAP_ESC_MAX_MOTOR_NUM];
uint8_t motor_cnt = _channels_count;
@ -578,7 +576,7 @@ void RCOutput_Tap::push()
_next_channel_reply = (_next_channel_reply + 1) % _channels_count;
perf_end(_perf_rcout);
hal.util->perf_end(_perf_rcout);
}
#endif

View File

@ -49,11 +49,9 @@
*/
#pragma once
#include "AP_HAL_PX4.h"
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_armed.h>
#include <AP_HAL/Util.h>
namespace PX4 {
namespace ap {
struct EscPacket;
@ -86,7 +84,7 @@ private:
bool _uart_open();
void _uart_close();
perf_counter_t _perf_rcout;
AP_HAL::Util::perf_counter_t _perf_rcout;
uint8_t _enabled_channels;
bool _corking;

View File

@ -2,6 +2,7 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include <AP_HAL/utility/RCOutput_Tap.h>
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
@ -13,7 +14,6 @@
#include "Storage.h"
#include "RCInput.h"
#include "RCOutput.h"
#include "RCOutput_Tap.h"
#include "AnalogIn.h"
#include "Util.h"
#include "GPIO.h"
@ -34,6 +34,7 @@
#include <drivers/drv_hrt.h>
using namespace PX4;
using namespace ap;
//static Empty::GPIO gpioDriver;