mirror of https://github.com/ArduPilot/ardupilot
parent
492c733750
commit
655f57ad01
|
@ -3,16 +3,16 @@
|
||||||
#define __AP_HAL_SITL_NAMESPACE_H__
|
#define __AP_HAL_SITL_NAMESPACE_H__
|
||||||
|
|
||||||
namespace HALSITL {
|
namespace HALSITL {
|
||||||
class SITLUARTDriver;
|
class UARTDriver;
|
||||||
class SITLScheduler;
|
class Scheduler;
|
||||||
class SITL_State;
|
class SITL_State;
|
||||||
class SITLEEPROMStorage;
|
class EEPROMStorage;
|
||||||
class SITLAnalogIn;
|
class AnalogIn;
|
||||||
class SITLRCInput;
|
class RCInput;
|
||||||
class SITLRCOutput;
|
class RCOutput;
|
||||||
class ADCSource;
|
class ADCSource;
|
||||||
class RCInput;
|
class RCInput;
|
||||||
class SITLUtil;
|
class Util;
|
||||||
class Semaphore;
|
class Semaphore;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -55,10 +55,10 @@ void ADCSource::set_pin(uint8_t pin) {
|
||||||
_pin = pin;
|
_pin = pin;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLAnalogIn::init() {
|
void AnalogIn::init() {
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t pin) {
|
AP_HAL::AnalogSource* AnalogIn::channel(int16_t pin) {
|
||||||
return new ADCSource(_sitlState, pin);
|
return new ADCSource(_sitlState, pin);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
|
|
||||||
class HALSITL::ADCSource : public AP_HAL::AnalogSource {
|
class HALSITL::ADCSource : public AP_HAL::AnalogSource {
|
||||||
public:
|
public:
|
||||||
friend class HALSITL::SITLAnalogIn;
|
friend class HALSITL::AnalogIn;
|
||||||
/* pin designates the ADC input number */
|
/* pin designates the ADC input number */
|
||||||
ADCSource(SITL_State *sitlState, uint8_t pin);
|
ADCSource(SITL_State *sitlState, uint8_t pin);
|
||||||
|
|
||||||
|
@ -30,11 +30,11 @@ private:
|
||||||
uint8_t _pin;
|
uint8_t _pin;
|
||||||
};
|
};
|
||||||
|
|
||||||
/* SITLAnalogIn : a concrete class providing the implementations of the
|
/* AnalogIn : a concrete class providing the implementations of the
|
||||||
* timer event and the AP_HAL::AnalogIn interface */
|
* timer event and the AP_HAL::AnalogIn interface */
|
||||||
class HALSITL::SITLAnalogIn : public AP_HAL::AnalogIn {
|
class HALSITL::AnalogIn : public AP_HAL::AnalogIn {
|
||||||
public:
|
public:
|
||||||
SITLAnalogIn(SITL_State *sitlState) {
|
AnalogIn(SITL_State *sitlState) {
|
||||||
_sitlState = sitlState;
|
_sitlState = sitlState;
|
||||||
}
|
}
|
||||||
void init();
|
void init();
|
||||||
|
|
|
@ -23,12 +23,12 @@
|
||||||
|
|
||||||
using namespace HALSITL;
|
using namespace HALSITL;
|
||||||
|
|
||||||
static SITLEEPROMStorage sitlEEPROMStorage;
|
static EEPROMStorage sitlEEPROMStorage;
|
||||||
static SITL_State sitlState;
|
static SITL_State sitlState;
|
||||||
static SITLScheduler sitlScheduler(&sitlState);
|
static Scheduler sitlScheduler(&sitlState);
|
||||||
static SITLRCInput sitlRCInput(&sitlState);
|
static RCInput sitlRCInput(&sitlState);
|
||||||
static SITLRCOutput sitlRCOutput(&sitlState);
|
static RCOutput sitlRCOutput(&sitlState);
|
||||||
static SITLAnalogIn sitlAnalogIn(&sitlState);
|
static AnalogIn sitlAnalogIn(&sitlState);
|
||||||
|
|
||||||
// use the Empty HAL for hardware we don't emulate
|
// use the Empty HAL for hardware we don't emulate
|
||||||
static Empty::GPIO emptyGPIO;
|
static Empty::GPIO emptyGPIO;
|
||||||
|
@ -37,13 +37,13 @@ static Empty::I2CDriver emptyI2C(&emptyI2Csemaphore);
|
||||||
static Empty::SPIDeviceManager emptySPI;
|
static Empty::SPIDeviceManager emptySPI;
|
||||||
static Empty::OpticalFlow emptyOpticalFlow;
|
static Empty::OpticalFlow emptyOpticalFlow;
|
||||||
|
|
||||||
static SITLUARTDriver sitlUart0Driver(0, &sitlState);
|
static UARTDriver sitlUart0Driver(0, &sitlState);
|
||||||
static SITLUARTDriver sitlUart1Driver(1, &sitlState);
|
static UARTDriver sitlUart1Driver(1, &sitlState);
|
||||||
static SITLUARTDriver sitlUart2Driver(2, &sitlState);
|
static UARTDriver sitlUart2Driver(2, &sitlState);
|
||||||
static SITLUARTDriver sitlUart3Driver(3, &sitlState);
|
static UARTDriver sitlUart3Driver(3, &sitlState);
|
||||||
static SITLUARTDriver sitlUart4Driver(4, &sitlState);
|
static UARTDriver sitlUart4Driver(4, &sitlState);
|
||||||
|
|
||||||
static SITLUtil utilInstance(&sitlState);
|
static Util utilInstance(&sitlState);
|
||||||
|
|
||||||
HAL_SITL::HAL_SITL() :
|
HAL_SITL::HAL_SITL() :
|
||||||
AP_HAL::HAL(
|
AP_HAL::HAL(
|
||||||
|
|
|
@ -7,12 +7,12 @@ using namespace HALSITL;
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
void SITLRCInput::init()
|
void RCInput::init()
|
||||||
{
|
{
|
||||||
clear_overrides();
|
clear_overrides();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SITLRCInput::new_input()
|
bool RCInput::new_input()
|
||||||
{
|
{
|
||||||
if (_sitlState->new_rc_input) {
|
if (_sitlState->new_rc_input) {
|
||||||
_sitlState->new_rc_input = false;
|
_sitlState->new_rc_input = false;
|
||||||
|
@ -21,7 +21,7 @@ bool SITLRCInput::new_input()
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t SITLRCInput::read(uint8_t ch)
|
uint16_t RCInput::read(uint8_t ch)
|
||||||
{
|
{
|
||||||
if (ch >= SITL_RC_INPUT_CHANNELS) {
|
if (ch >= SITL_RC_INPUT_CHANNELS) {
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -29,7 +29,7 @@ uint16_t SITLRCInput::read(uint8_t ch)
|
||||||
return _override[ch]? _override[ch] : _sitlState->pwm_input[ch];
|
return _override[ch]? _override[ch] : _sitlState->pwm_input[ch];
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len)
|
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
|
||||||
{
|
{
|
||||||
if (len > SITL_RC_INPUT_CHANNELS) {
|
if (len > SITL_RC_INPUT_CHANNELS) {
|
||||||
len = SITL_RC_INPUT_CHANNELS;
|
len = SITL_RC_INPUT_CHANNELS;
|
||||||
|
@ -40,7 +40,7 @@ uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len)
|
||||||
return 8;
|
return 8;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len)
|
bool RCInput::set_overrides(int16_t *overrides, uint8_t len)
|
||||||
{
|
{
|
||||||
bool res = false;
|
bool res = false;
|
||||||
if (len > SITL_RC_INPUT_CHANNELS) {
|
if (len > SITL_RC_INPUT_CHANNELS) {
|
||||||
|
@ -52,7 +52,7 @@ bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len)
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SITLRCInput::set_override(uint8_t channel, int16_t override)
|
bool RCInput::set_override(uint8_t channel, int16_t override)
|
||||||
{
|
{
|
||||||
if (override < 0) return false; /* -1: no change. */
|
if (override < 0) return false; /* -1: no change. */
|
||||||
if (channel < SITL_RC_INPUT_CHANNELS) {
|
if (channel < SITL_RC_INPUT_CHANNELS) {
|
||||||
|
@ -64,7 +64,7 @@ bool SITLRCInput::set_override(uint8_t channel, int16_t override)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLRCInput::clear_overrides()
|
void RCInput::clear_overrides()
|
||||||
{
|
{
|
||||||
memset(_override, 0, sizeof(_override));
|
memset(_override, 0, sizeof(_override));
|
||||||
}
|
}
|
||||||
|
|
|
@ -7,9 +7,9 @@
|
||||||
|
|
||||||
#include "AP_HAL_SITL.h"
|
#include "AP_HAL_SITL.h"
|
||||||
|
|
||||||
class HALSITL::SITLRCInput : public AP_HAL::RCInput {
|
class HALSITL::RCInput : public AP_HAL::RCInput {
|
||||||
public:
|
public:
|
||||||
SITLRCInput(SITL_State *sitlState) {
|
RCInput(SITL_State *sitlState) {
|
||||||
_sitlState = sitlState;
|
_sitlState = sitlState;
|
||||||
}
|
}
|
||||||
void init() override;
|
void init() override;
|
||||||
|
|
|
@ -14,20 +14,20 @@
|
||||||
|
|
||||||
using namespace HALSITL;
|
using namespace HALSITL;
|
||||||
|
|
||||||
void SITLRCOutput::init() {}
|
void RCOutput::init() {}
|
||||||
|
|
||||||
void SITLRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
||||||
{
|
{
|
||||||
Debug("set_freq(0x%04x, %u)\n", (unsigned)chmask, (unsigned)freq_hz);
|
Debug("set_freq(0x%04x, %u)\n", (unsigned)chmask, (unsigned)freq_hz);
|
||||||
_freq_hz = freq_hz;
|
_freq_hz = freq_hz;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t SITLRCOutput::get_freq(uint8_t ch)
|
uint16_t RCOutput::get_freq(uint8_t ch)
|
||||||
{
|
{
|
||||||
return _freq_hz;
|
return _freq_hz;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLRCOutput::enable_ch(uint8_t ch)
|
void RCOutput::enable_ch(uint8_t ch)
|
||||||
{
|
{
|
||||||
if (!(_enable_mask & (1U<<ch))) {
|
if (!(_enable_mask & (1U<<ch))) {
|
||||||
Debug("enable_ch(%u)\n", ch);
|
Debug("enable_ch(%u)\n", ch);
|
||||||
|
@ -35,7 +35,7 @@ void SITLRCOutput::enable_ch(uint8_t ch)
|
||||||
_enable_mask |= 1U<<ch;
|
_enable_mask |= 1U<<ch;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLRCOutput::disable_ch(uint8_t ch)
|
void RCOutput::disable_ch(uint8_t ch)
|
||||||
{
|
{
|
||||||
if (_enable_mask & (1U<<ch)) {
|
if (_enable_mask & (1U<<ch)) {
|
||||||
Debug("disable_ch(%u)\n", ch);
|
Debug("disable_ch(%u)\n", ch);
|
||||||
|
@ -43,14 +43,14 @@ void SITLRCOutput::disable_ch(uint8_t ch)
|
||||||
_enable_mask &= ~1U<<ch;
|
_enable_mask &= ~1U<<ch;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLRCOutput::write(uint8_t ch, uint16_t period_us)
|
void RCOutput::write(uint8_t ch, uint16_t period_us)
|
||||||
{
|
{
|
||||||
if (ch < SITL_NUM_CHANNELS) {
|
if (ch < SITL_NUM_CHANNELS) {
|
||||||
_sitlState->pwm_output[ch] = period_us;
|
_sitlState->pwm_output[ch] = period_us;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t SITLRCOutput::read(uint8_t ch)
|
uint16_t RCOutput::read(uint8_t ch)
|
||||||
{
|
{
|
||||||
if (ch < SITL_NUM_CHANNELS) {
|
if (ch < SITL_NUM_CHANNELS) {
|
||||||
return _sitlState->pwm_output[ch];
|
return _sitlState->pwm_output[ch];
|
||||||
|
@ -58,7 +58,7 @@ uint16_t SITLRCOutput::read(uint8_t ch)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLRCOutput::read(uint16_t* period_us, uint8_t len)
|
void RCOutput::read(uint16_t* period_us, uint8_t len)
|
||||||
{
|
{
|
||||||
memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
|
memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
|
||||||
}
|
}
|
||||||
|
|
|
@ -6,9 +6,9 @@
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
#include "AP_HAL_SITL.h"
|
#include "AP_HAL_SITL.h"
|
||||||
|
|
||||||
class HALSITL::SITLRCOutput : public AP_HAL::RCOutput {
|
class HALSITL::RCOutput : public AP_HAL::RCOutput {
|
||||||
public:
|
public:
|
||||||
SITLRCOutput(SITL_State *sitlState) {
|
RCOutput(SITL_State *sitlState) {
|
||||||
_sitlState = sitlState;
|
_sitlState = sitlState;
|
||||||
_freq_hz = 50;
|
_freq_hz = 50;
|
||||||
}
|
}
|
||||||
|
|
|
@ -446,7 +446,7 @@ void SITL_State::init(int argc, char * const argv[])
|
||||||
pwm_input[4] = pwm_input[7] = 1800;
|
pwm_input[4] = pwm_input[7] = 1800;
|
||||||
pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
|
pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
|
||||||
|
|
||||||
_scheduler = SITLScheduler::from(hal.scheduler);
|
_scheduler = Scheduler::from(hal.scheduler);
|
||||||
_parse_command_line(argc, argv);
|
_parse_command_line(argc, argv);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,8 +30,8 @@
|
||||||
class HAL_SITL;
|
class HAL_SITL;
|
||||||
|
|
||||||
class HALSITL::SITL_State {
|
class HALSITL::SITL_State {
|
||||||
friend class HALSITL::SITLScheduler;
|
friend class HALSITL::Scheduler;
|
||||||
friend class HALSITL::SITLUtil;
|
friend class HALSITL::Util;
|
||||||
public:
|
public:
|
||||||
void init(int argc, char * const argv[]);
|
void init(int argc, char * const argv[]);
|
||||||
|
|
||||||
|
@ -145,7 +145,7 @@ private:
|
||||||
|
|
||||||
AP_Baro *_barometer;
|
AP_Baro *_barometer;
|
||||||
AP_InertialSensor *_ins;
|
AP_InertialSensor *_ins;
|
||||||
SITLScheduler *_scheduler;
|
Scheduler *_scheduler;
|
||||||
Compass *_compass;
|
Compass *_compass;
|
||||||
OpticalFlow *_optical_flow;
|
OpticalFlow *_optical_flow;
|
||||||
AP_Terrain *_terrain;
|
AP_Terrain *_terrain;
|
||||||
|
|
|
@ -163,7 +163,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
|
||||||
_framerate = (unsigned)atoi(gopt.optarg);
|
_framerate = (unsigned)atoi(gopt.optarg);
|
||||||
break;
|
break;
|
||||||
case 'C':
|
case 'C':
|
||||||
HALSITL::SITLUARTDriver::_console = true;
|
HALSITL::UARTDriver::_console = true;
|
||||||
break;
|
break;
|
||||||
case 'I': {
|
case 'I': {
|
||||||
_instance = atoi(gopt.optarg);
|
_instance = atoi(gopt.optarg);
|
||||||
|
|
|
@ -16,29 +16,29 @@ using namespace HALSITL;
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
|
||||||
AP_HAL::Proc SITLScheduler::_failsafe = NULL;
|
AP_HAL::Proc Scheduler::_failsafe = NULL;
|
||||||
volatile bool SITLScheduler::_timer_suspended = false;
|
volatile bool Scheduler::_timer_suspended = false;
|
||||||
volatile bool SITLScheduler::_timer_event_missed = false;
|
volatile bool Scheduler::_timer_event_missed = false;
|
||||||
|
|
||||||
AP_HAL::MemberProc SITLScheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
AP_HAL::MemberProc Scheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
||||||
uint8_t SITLScheduler::_num_timer_procs = 0;
|
uint8_t Scheduler::_num_timer_procs = 0;
|
||||||
bool SITLScheduler::_in_timer_proc = false;
|
bool Scheduler::_in_timer_proc = false;
|
||||||
|
|
||||||
AP_HAL::MemberProc SITLScheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
||||||
uint8_t SITLScheduler::_num_io_procs = 0;
|
uint8_t Scheduler::_num_io_procs = 0;
|
||||||
bool SITLScheduler::_in_io_proc = false;
|
bool Scheduler::_in_io_proc = false;
|
||||||
|
|
||||||
SITLScheduler::SITLScheduler(SITL_State *sitlState) :
|
Scheduler::Scheduler(SITL_State *sitlState) :
|
||||||
_sitlState(sitlState),
|
_sitlState(sitlState),
|
||||||
_stopped_clock_usec(0)
|
_stopped_clock_usec(0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::init()
|
void Scheduler::init()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::delay_microseconds(uint16_t usec)
|
void Scheduler::delay_microseconds(uint16_t usec)
|
||||||
{
|
{
|
||||||
uint64_t start = AP_HAL::micros64();
|
uint64_t start = AP_HAL::micros64();
|
||||||
uint64_t dtime;
|
uint64_t dtime;
|
||||||
|
@ -51,7 +51,7 @@ void SITLScheduler::delay_microseconds(uint16_t usec)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::delay(uint16_t ms)
|
void Scheduler::delay(uint16_t ms)
|
||||||
{
|
{
|
||||||
while (ms > 0) {
|
while (ms > 0) {
|
||||||
delay_microseconds(1000);
|
delay_microseconds(1000);
|
||||||
|
@ -64,14 +64,14 @@ void SITLScheduler::delay(uint16_t ms)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::register_delay_callback(AP_HAL::Proc proc,
|
void Scheduler::register_delay_callback(AP_HAL::Proc proc,
|
||||||
uint16_t min_time_ms)
|
uint16_t min_time_ms)
|
||||||
{
|
{
|
||||||
_delay_cb = proc;
|
_delay_cb = proc;
|
||||||
_min_delay_cb_ms = min_time_ms;
|
_min_delay_cb_ms = min_time_ms;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc)
|
void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < _num_timer_procs; i++) {
|
for (uint8_t i = 0; i < _num_timer_procs; i++) {
|
||||||
if (_timer_proc[i] == proc) {
|
if (_timer_proc[i] == proc) {
|
||||||
|
@ -86,7 +86,7 @@ void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::register_io_process(AP_HAL::MemberProc proc)
|
void Scheduler::register_io_process(AP_HAL::MemberProc proc)
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < _num_io_procs; i++) {
|
for (uint8_t i = 0; i < _num_io_procs; i++) {
|
||||||
if (_io_proc[i] == proc) {
|
if (_io_proc[i] == proc) {
|
||||||
|
@ -101,16 +101,16 @@ void SITLScheduler::register_io_process(AP_HAL::MemberProc proc)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
|
void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
|
||||||
{
|
{
|
||||||
_failsafe = failsafe;
|
_failsafe = failsafe;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::suspend_timer_procs() {
|
void Scheduler::suspend_timer_procs() {
|
||||||
_timer_suspended = true;
|
_timer_suspended = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::resume_timer_procs() {
|
void Scheduler::resume_timer_procs() {
|
||||||
_timer_suspended = false;
|
_timer_suspended = false;
|
||||||
if (_timer_event_missed) {
|
if (_timer_event_missed) {
|
||||||
_timer_event_missed = false;
|
_timer_event_missed = false;
|
||||||
|
@ -118,15 +118,15 @@ void SITLScheduler::resume_timer_procs() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SITLScheduler::in_timerprocess() {
|
bool Scheduler::in_timerprocess() {
|
||||||
return _in_timer_proc || _in_io_proc;
|
return _in_timer_proc || _in_io_proc;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SITLScheduler::system_initializing() {
|
bool Scheduler::system_initializing() {
|
||||||
return !_initialized;
|
return !_initialized;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::system_initialized() {
|
void Scheduler::system_initialized() {
|
||||||
if (_initialized) {
|
if (_initialized) {
|
||||||
AP_HAL::panic(
|
AP_HAL::panic(
|
||||||
"PANIC: scheduler system initialized called more than once");
|
"PANIC: scheduler system initialized called more than once");
|
||||||
|
@ -144,19 +144,19 @@ void SITLScheduler::system_initialized() {
|
||||||
_initialized = true;
|
_initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::sitl_end_atomic() {
|
void Scheduler::sitl_end_atomic() {
|
||||||
if (_nested_atomic_ctr == 0)
|
if (_nested_atomic_ctr == 0)
|
||||||
hal.uartA->println("NESTED ATOMIC ERROR");
|
hal.uartA->println("NESTED ATOMIC ERROR");
|
||||||
else
|
else
|
||||||
_nested_atomic_ctr--;
|
_nested_atomic_ctr--;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::reboot(bool hold_in_bootloader)
|
void Scheduler::reboot(bool hold_in_bootloader)
|
||||||
{
|
{
|
||||||
hal.uartA->println("REBOOT NOT IMPLEMENTED\r\n");
|
hal.uartA->println("REBOOT NOT IMPLEMENTED\r\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::_run_timer_procs(bool called_from_isr)
|
void Scheduler::_run_timer_procs(bool called_from_isr)
|
||||||
{
|
{
|
||||||
if (_in_timer_proc) {
|
if (_in_timer_proc) {
|
||||||
// the timer calls took longer than the period of the
|
// the timer calls took longer than the period of the
|
||||||
|
@ -195,7 +195,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
|
||||||
_in_timer_proc = false;
|
_in_timer_proc = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLScheduler::_run_io_procs(bool called_from_isr)
|
void Scheduler::_run_io_procs(bool called_from_isr)
|
||||||
{
|
{
|
||||||
if (_in_io_proc) {
|
if (_in_io_proc) {
|
||||||
return;
|
return;
|
||||||
|
@ -215,17 +215,17 @@ void SITLScheduler::_run_io_procs(bool called_from_isr)
|
||||||
|
|
||||||
_in_io_proc = false;
|
_in_io_proc = false;
|
||||||
|
|
||||||
SITLUARTDriver::from(hal.uartA)->_timer_tick();
|
UARTDriver::from(hal.uartA)->_timer_tick();
|
||||||
SITLUARTDriver::from(hal.uartB)->_timer_tick();
|
UARTDriver::from(hal.uartB)->_timer_tick();
|
||||||
SITLUARTDriver::from(hal.uartC)->_timer_tick();
|
UARTDriver::from(hal.uartC)->_timer_tick();
|
||||||
SITLUARTDriver::from(hal.uartD)->_timer_tick();
|
UARTDriver::from(hal.uartD)->_timer_tick();
|
||||||
SITLUARTDriver::from(hal.uartE)->_timer_tick();
|
UARTDriver::from(hal.uartE)->_timer_tick();
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
set simulation timestamp
|
set simulation timestamp
|
||||||
*/
|
*/
|
||||||
void SITLScheduler::stop_clock(uint64_t time_usec)
|
void Scheduler::stop_clock(uint64_t time_usec)
|
||||||
{
|
{
|
||||||
_stopped_clock_usec = time_usec;
|
_stopped_clock_usec = time_usec;
|
||||||
_run_io_procs(false);
|
_run_io_procs(false);
|
||||||
|
|
|
@ -1,6 +1,4 @@
|
||||||
|
#pragma once
|
||||||
#ifndef __AP_HAL_SITL_SCHEDULER_H__
|
|
||||||
#define __AP_HAL_SITL_SCHEDULER_H__
|
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
@ -10,11 +8,11 @@
|
||||||
#define SITL_SCHEDULER_MAX_TIMER_PROCS 4
|
#define SITL_SCHEDULER_MAX_TIMER_PROCS 4
|
||||||
|
|
||||||
/* Scheduler implementation: */
|
/* Scheduler implementation: */
|
||||||
class HALSITL::SITLScheduler : public AP_HAL::Scheduler {
|
class HALSITL::Scheduler : public AP_HAL::Scheduler {
|
||||||
public:
|
public:
|
||||||
SITLScheduler(SITL_State *sitlState);
|
Scheduler(SITL_State *sitlState);
|
||||||
static SITLScheduler *from(AP_HAL::Scheduler *scheduler) {
|
static Scheduler *from(AP_HAL::Scheduler *scheduler) {
|
||||||
return static_cast<SITLScheduler*>(scheduler);
|
return static_cast<HALSITL::Scheduler*>(scheduler);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* AP_HAL::Scheduler methods */
|
/* AP_HAL::Scheduler methods */
|
||||||
|
@ -78,7 +76,8 @@ private:
|
||||||
bool _initialized;
|
bool _initialized;
|
||||||
uint64_t _stopped_clock_usec;
|
uint64_t _stopped_clock_usec;
|
||||||
};
|
};
|
||||||
#endif
|
#endif // CONFIG_HAL_BOARD
|
||||||
#endif // __AP_HAL_SITL_SCHEDULER_H__
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -10,7 +10,7 @@
|
||||||
#include "Storage.h"
|
#include "Storage.h"
|
||||||
using namespace HALSITL;
|
using namespace HALSITL;
|
||||||
|
|
||||||
void SITLEEPROMStorage::_eeprom_open(void)
|
void EEPROMStorage::_eeprom_open(void)
|
||||||
{
|
{
|
||||||
if (_eeprom_fd == -1) {
|
if (_eeprom_fd == -1) {
|
||||||
_eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT, 0777);
|
_eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT, 0777);
|
||||||
|
@ -18,14 +18,14 @@ void SITLEEPROMStorage::_eeprom_open(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLEEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
|
void EEPROMStorage::read_block(void *dst, uint16_t src, size_t n)
|
||||||
{
|
{
|
||||||
assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE);
|
assert(src < HAL_STORAGE_SIZE && src + n <= HAL_STORAGE_SIZE);
|
||||||
_eeprom_open();
|
_eeprom_open();
|
||||||
assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
|
assert(pread(_eeprom_fd, dst, n, src) == (ssize_t)n);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLEEPROMStorage::write_block(uint16_t dst, const void *src, size_t n)
|
void EEPROMStorage::write_block(uint16_t dst, const void *src, size_t n)
|
||||||
{
|
{
|
||||||
assert(dst < HAL_STORAGE_SIZE);
|
assert(dst < HAL_STORAGE_SIZE);
|
||||||
_eeprom_open();
|
_eeprom_open();
|
||||||
|
|
|
@ -6,9 +6,9 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include "AP_HAL_SITL_Namespace.h"
|
#include "AP_HAL_SITL_Namespace.h"
|
||||||
|
|
||||||
class HALSITL::SITLEEPROMStorage : public AP_HAL::Storage {
|
class HALSITL::EEPROMStorage : public AP_HAL::Storage {
|
||||||
public:
|
public:
|
||||||
SITLEEPROMStorage() {
|
EEPROMStorage() {
|
||||||
_eeprom_fd = -1;
|
_eeprom_fd = -1;
|
||||||
}
|
}
|
||||||
void init() {}
|
void init() {}
|
||||||
|
|
|
@ -44,11 +44,11 @@ extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
using namespace HALSITL;
|
using namespace HALSITL;
|
||||||
|
|
||||||
bool SITLUARTDriver::_console;
|
bool UARTDriver::_console;
|
||||||
|
|
||||||
/* UARTDriver method implementations */
|
/* UARTDriver method implementations */
|
||||||
|
|
||||||
void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
|
void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
|
||||||
{
|
{
|
||||||
const char *path = _sitlState->_uart_path[_portNumber];
|
const char *path = _sitlState->_uart_path[_portNumber];
|
||||||
|
|
||||||
|
@ -96,11 +96,11 @@ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
|
||||||
_set_nonblocking(_fd);
|
_set_nonblocking(_fd);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLUARTDriver::end()
|
void UARTDriver::end()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t SITLUARTDriver::available(void)
|
int16_t UARTDriver::available(void)
|
||||||
{
|
{
|
||||||
_check_connection();
|
_check_connection();
|
||||||
|
|
||||||
|
@ -113,7 +113,7 @@ int16_t SITLUARTDriver::available(void)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int16_t SITLUARTDriver::txspace(void)
|
int16_t UARTDriver::txspace(void)
|
||||||
{
|
{
|
||||||
_check_connection();
|
_check_connection();
|
||||||
if (!_connected) {
|
if (!_connected) {
|
||||||
|
@ -122,7 +122,7 @@ int16_t SITLUARTDriver::txspace(void)
|
||||||
return _writebuffer.space();
|
return _writebuffer.space();
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t SITLUARTDriver::read(void)
|
int16_t UARTDriver::read(void)
|
||||||
{
|
{
|
||||||
if (available() <= 0) {
|
if (available() <= 0) {
|
||||||
return -1;
|
return -1;
|
||||||
|
@ -132,11 +132,11 @@ int16_t SITLUARTDriver::read(void)
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLUARTDriver::flush(void)
|
void UARTDriver::flush(void)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t SITLUARTDriver::write(uint8_t c)
|
size_t UARTDriver::write(uint8_t c)
|
||||||
{
|
{
|
||||||
if (txspace() <= 0) {
|
if (txspace() <= 0) {
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -145,7 +145,7 @@ size_t SITLUARTDriver::write(uint8_t c)
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size)
|
size_t UARTDriver::write(const uint8_t *buffer, size_t size)
|
||||||
{
|
{
|
||||||
if (txspace() <= (ssize_t)size) {
|
if (txspace() <= (ssize_t)size) {
|
||||||
size = txspace();
|
size = txspace();
|
||||||
|
@ -162,7 +162,7 @@ size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size)
|
||||||
start a TCP connection for the serial port. If wait_for_connection
|
start a TCP connection for the serial port. If wait_for_connection
|
||||||
is true then block until a client connects
|
is true then block until a client connects
|
||||||
*/
|
*/
|
||||||
void SITLUARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection)
|
void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection)
|
||||||
{
|
{
|
||||||
int one=1;
|
int one=1;
|
||||||
struct sockaddr_in sockaddr;
|
struct sockaddr_in sockaddr;
|
||||||
|
@ -250,7 +250,7 @@ void SITLUARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connecti
|
||||||
/*
|
/*
|
||||||
start a TCP client connection for the serial port.
|
start a TCP client connection for the serial port.
|
||||||
*/
|
*/
|
||||||
void SITLUARTDriver::_tcp_start_client(const char *address, uint16_t port)
|
void UARTDriver::_tcp_start_client(const char *address, uint16_t port)
|
||||||
{
|
{
|
||||||
int one=1;
|
int one=1;
|
||||||
struct sockaddr_in sockaddr;
|
struct sockaddr_in sockaddr;
|
||||||
|
@ -301,7 +301,7 @@ void SITLUARTDriver::_tcp_start_client(const char *address, uint16_t port)
|
||||||
/*
|
/*
|
||||||
start a UART connection for the serial port
|
start a UART connection for the serial port
|
||||||
*/
|
*/
|
||||||
void SITLUARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
|
void UARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
|
||||||
{
|
{
|
||||||
struct termios t {};
|
struct termios t {};
|
||||||
if (!_connected) {
|
if (!_connected) {
|
||||||
|
@ -338,7 +338,7 @@ void SITLUARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
|
||||||
/*
|
/*
|
||||||
see if a new connection is coming in
|
see if a new connection is coming in
|
||||||
*/
|
*/
|
||||||
void SITLUARTDriver::_check_connection(void)
|
void UARTDriver::_check_connection(void)
|
||||||
{
|
{
|
||||||
if (_connected) {
|
if (_connected) {
|
||||||
// we only want 1 connection at a time
|
// we only want 1 connection at a time
|
||||||
|
@ -359,7 +359,7 @@ void SITLUARTDriver::_check_connection(void)
|
||||||
/*
|
/*
|
||||||
use select() to see if something is pending
|
use select() to see if something is pending
|
||||||
*/
|
*/
|
||||||
bool SITLUARTDriver::_select_check(int fd)
|
bool UARTDriver::_select_check(int fd)
|
||||||
{
|
{
|
||||||
fd_set fds;
|
fd_set fds;
|
||||||
struct timeval tv;
|
struct timeval tv;
|
||||||
|
@ -377,13 +377,13 @@ bool SITLUARTDriver::_select_check(int fd)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLUARTDriver::_set_nonblocking(int fd)
|
void UARTDriver::_set_nonblocking(int fd)
|
||||||
{
|
{
|
||||||
unsigned v = fcntl(fd, F_GETFL, 0);
|
unsigned v = fcntl(fd, F_GETFL, 0);
|
||||||
fcntl(fd, F_SETFL, v | O_NONBLOCK);
|
fcntl(fd, F_SETFL, v | O_NONBLOCK);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SITLUARTDriver::_timer_tick(void)
|
void UARTDriver::_timer_tick(void)
|
||||||
{
|
{
|
||||||
uint32_t navail;
|
uint32_t navail;
|
||||||
ssize_t nwritten;
|
ssize_t nwritten;
|
||||||
|
|
|
@ -11,11 +11,11 @@
|
||||||
#include <AP_HAL/utility/Socket.h>
|
#include <AP_HAL/utility/Socket.h>
|
||||||
#include <AP_HAL/utility/RingBuffer.h>
|
#include <AP_HAL/utility/RingBuffer.h>
|
||||||
|
|
||||||
class HALSITL::SITLUARTDriver : public AP_HAL::UARTDriver {
|
class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
|
||||||
public:
|
public:
|
||||||
friend class HALSITL::SITL_State;
|
friend class HALSITL::SITL_State;
|
||||||
|
|
||||||
SITLUARTDriver(const uint8_t portNumber, SITL_State *sitlState) {
|
UARTDriver(const uint8_t portNumber, SITL_State *sitlState) {
|
||||||
_portNumber = portNumber;
|
_portNumber = portNumber;
|
||||||
_sitlState = sitlState;
|
_sitlState = sitlState;
|
||||||
|
|
||||||
|
@ -23,8 +23,8 @@ public:
|
||||||
_listen_fd = -1;
|
_listen_fd = -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static SITLUARTDriver *from(AP_HAL::UARTDriver *uart) {
|
static UARTDriver *from(AP_HAL::UARTDriver *uart) {
|
||||||
return static_cast<SITLUARTDriver*>(uart);
|
return static_cast<UARTDriver*>(uart);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Implementations of UARTDriver virtual methods */
|
/* Implementations of UARTDriver virtual methods */
|
||||||
|
|
|
@ -6,9 +6,9 @@
|
||||||
#include "AP_HAL_SITL_Namespace.h"
|
#include "AP_HAL_SITL_Namespace.h"
|
||||||
#include "Semaphores.h"
|
#include "Semaphores.h"
|
||||||
|
|
||||||
class HALSITL::SITLUtil : public AP_HAL::Util {
|
class HALSITL::Util : public AP_HAL::Util {
|
||||||
public:
|
public:
|
||||||
SITLUtil(SITL_State *_sitlState) :
|
Util(SITL_State *_sitlState) :
|
||||||
sitlState(_sitlState) {}
|
sitlState(_sitlState) {}
|
||||||
|
|
||||||
bool run_debug_shell(AP_HAL::BetterStream *stream) {
|
bool run_debug_shell(AP_HAL::BetterStream *stream) {
|
||||||
|
|
|
@ -76,8 +76,8 @@ int SITL_State::gps_pipe(void)
|
||||||
gps_state.gps_fd = fd[1];
|
gps_state.gps_fd = fd[1];
|
||||||
gps_state.client_fd = fd[0];
|
gps_state.client_fd = fd[0];
|
||||||
gps_state.last_update = AP_HAL::millis();
|
gps_state.last_update = AP_HAL::millis();
|
||||||
HALSITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd);
|
HALSITL::UARTDriver::_set_nonblocking(gps_state.gps_fd);
|
||||||
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
|
HALSITL::UARTDriver::_set_nonblocking(fd[0]);
|
||||||
return gps_state.client_fd;
|
return gps_state.client_fd;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -94,8 +94,8 @@ int SITL_State::gps2_pipe(void)
|
||||||
gps2_state.gps_fd = fd[1];
|
gps2_state.gps_fd = fd[1];
|
||||||
gps2_state.client_fd = fd[0];
|
gps2_state.client_fd = fd[0];
|
||||||
gps2_state.last_update = AP_HAL::millis();
|
gps2_state.last_update = AP_HAL::millis();
|
||||||
HALSITL::SITLUARTDriver::_set_nonblocking(gps2_state.gps_fd);
|
HALSITL::UARTDriver::_set_nonblocking(gps2_state.gps_fd);
|
||||||
HALSITL::SITLUARTDriver::_set_nonblocking(fd[0]);
|
HALSITL::UARTDriver::_set_nonblocking(fd[0]);
|
||||||
return gps2_state.client_fd;
|
return gps2_state.client_fd;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -8,7 +8,7 @@
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
using HALSITL::SITLScheduler;
|
using HALSITL::Scheduler;
|
||||||
|
|
||||||
namespace AP_HAL {
|
namespace AP_HAL {
|
||||||
|
|
||||||
|
@ -45,7 +45,7 @@ uint32_t millis()
|
||||||
|
|
||||||
uint64_t micros64()
|
uint64_t micros64()
|
||||||
{
|
{
|
||||||
const SITLScheduler* scheduler = SITLScheduler::from(hal.scheduler);
|
const HALSITL::Scheduler* scheduler = HALSITL::Scheduler::from(hal.scheduler);
|
||||||
uint64_t stopped_usec = scheduler->stopped_clock_usec();
|
uint64_t stopped_usec = scheduler->stopped_clock_usec();
|
||||||
if (stopped_usec) {
|
if (stopped_usec) {
|
||||||
return stopped_usec;
|
return stopped_usec;
|
||||||
|
@ -61,7 +61,7 @@ uint64_t micros64()
|
||||||
|
|
||||||
uint64_t millis64()
|
uint64_t millis64()
|
||||||
{
|
{
|
||||||
const SITLScheduler* scheduler = SITLScheduler::from(hal.scheduler);
|
const HALSITL::Scheduler* scheduler = HALSITL::Scheduler::from(hal.scheduler);
|
||||||
uint64_t stopped_usec = scheduler->stopped_clock_usec();
|
uint64_t stopped_usec = scheduler->stopped_clock_usec();
|
||||||
if (stopped_usec) {
|
if (stopped_usec) {
|
||||||
return stopped_usec / 1000;
|
return stopped_usec / 1000;
|
||||||
|
|
Loading…
Reference in New Issue