aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: added RCInput and RCOutput

Browse Source
This commit is contained in:
Andrew Tridgell 2012-12-18 14:56:21 +11:00
parent 28f0ce6185
commit 0c9d37e2ee
9 changed files with 183 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h
View File

@ -8,8 +8,10 @@ namespace AVR_SITL {
class SITL_State;
class SITLConsoleDriver;
class SITLEEPROMStorage;
class SITLAnalogIn;
class ADCSource;
class SITLAnalogIn;
class SITLRCInput;
class SITLRCOutput;
class ADCSource;
class RCInput;
}

11
libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp
View File

@ -15,6 +15,7 @@
#include "Storage.h"
#include "Console.h"
#include "RCInput.h"
#include "RCOutput.h"
#include "SITL_State.h"
#include <AP_HAL_Empty.h>
@ -26,9 +27,9 @@ static SITLScheduler sitlScheduler;
static SITLEEPROMStorage sitlEEPROMStorage;
static SITLConsoleDriver consoleDriver;
static SITL_State sitlState;
static SITLRCInput sitlRCInput(&sitlState);
static SITLRCOutput sitlRCOutput(&sitlState);
static Empty::EmptyRCInput emptyRCInput;
static Empty::EmptyRCOutput emptyRCOutput;
static Empty::EmptyGPIO emptyGPIO;
static Empty::EmptyAnalogIn emptyAnalogIn;
@ -46,11 +47,9 @@ HAL_AVR_SITL::HAL_AVR_SITL() :
&emptyAnalogIn, /* analogin */
&sitlEEPROMStorage, /* storage */
&consoleDriver, /* console */
&emptyGPIO, /* gpio */
&emptyRCInput, /* rcinput */
&emptyRCOutput, /* rcoutput */
&sitlRCInput, /* rcinput */
&sitlRCOutput, /* rcoutput */
&sitlScheduler), /* scheduler */
_sitl_state(&sitlState)
{}

52
libraries/AP_HAL_AVR_SITL/RCInput.cpp Normal file
View File

@ -0,0 +1,52 @@
#include "RCInput.h"
using namespace AVR_SITL;
extern const AP_HAL::HAL& hal;
void SITLRCInput::init(void* machtnichts)
{
clear_overrides();
}
uint8_t SITLRCInput::valid() {
return 0;
}
uint16_t SITLRCInput::read(uint8_t ch) {
return _override[ch]? _override[ch] : _sitlState->pwm_input[ch];
}
uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) {
for (uint8_t i=0; i<len; i++) {
periods[i] = _override[i]? _override[i] : _sitlState->pwm_input[i];
}
return len;
}
bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len) {
bool res = false;
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
if (override < 0) return false; /* -1: no change. */
if (channel < 8) {
_override[channel] = override;
if (override != 0) {
return true;
}
}
return false;
}
void SITLRCInput::clear_overrides()
{
for (uint8_t i = 0; i < 8; i++) {
_override[i] = 0;
}
}

28
libraries/AP_HAL_AVR_SITL/RCInput.h Normal file
View File

@ -0,0 +1,28 @@
#ifndef __AP_HAL_AVR_SITL_RCINPUT_H__
#define __AP_HAL_AVR_SITL_RCINPUT_H__
#include <AP_HAL_AVR_SITL.h>
class AVR_SITL::SITLRCInput : public AP_HAL::RCInput {
public:
SITLRCInput(SITL_State *sitlState) {
_sitlState = sitlState;
}
void init(void* machtnichts);
uint8_t valid();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
private:
SITL_State *_sitlState;
/* override state */
uint16_t _override[8];
};
#endif // __AP_HAL_AVR_SITL_RCINPUT_H__

45
libraries/AP_HAL_AVR_SITL/RCOutput.cpp Normal file
View File

@ -0,0 +1,45 @@
#include "RCOutput.h"
using namespace AVR_SITL;
void SITLRCOutput::init(void* machtnichts) {}
void SITLRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {
_freq_hz = freq_hz;
}
uint16_t SITLRCOutput::get_freq(uint8_t ch) {
return _freq_hz;
}
void SITLRCOutput::enable_ch(uint8_t ch)
{}
void SITLRCOutput::enable_mask(uint32_t chmask)
{}
void SITLRCOutput::disable_ch(uint8_t ch)
{}
void SITLRCOutput::disable_mask(uint32_t chmask)
{}
void SITLRCOutput::write(uint8_t ch, uint16_t period_us)
{
_sitlState->pwm_output[ch] = period_us;
}
void SITLRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
{
memcpy(_sitlState->pwm_output+ch, period_us, len*sizeof(uint16_t));
}
uint16_t SITLRCOutput::read(uint8_t ch) {
return _sitlState->pwm_output[ch];
}
void SITLRCOutput::read(uint16_t* period_us, uint8_t len)
{
memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t));
}

31
libraries/AP_HAL_AVR_SITL/RCOutput.h Normal file
View File

@ -0,0 +1,31 @@
#ifndef __AP_HAL_AVR_SITL_RCOUTPUT_H__
#define __AP_HAL_AVR_SITL_RCOUTPUT_H__
#include <AP_HAL_AVR_SITL.h>
class AVR_SITL::SITLRCOutput : public AP_HAL::RCOutput {
public:
SITLRCOutput(SITL_State *sitlState) {
_sitlState = sitlState;
_freq_hz = 50;
}
void init(void* machtnichts);
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void enable_mask(uint32_t chmask);
void disable_ch(uint8_t ch);
void disable_mask(uint32_t chmask);
void write(uint8_t ch, uint16_t period_us);
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
private:
SITL_State *_sitlState;
uint16_t _freq_hz;
};
#endif // __AP_HAL_AVR_SITL_RCOUTPUT_H__

31
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -22,9 +22,6 @@
extern const AP_HAL::HAL& hal;
// emulate RC input
struct RC_ICR4 ICR4;
using namespace AVR_SITL;
enum SITL_State::vehicle_type SITL_State::_vehicle;
@ -41,7 +38,8 @@ AP_Compass_HIL *SITL_State::_compass;
int SITL_State::_sitl_fd;
SITL *SITL_State::_sitl;
uint16_t SITL_State::_pwm_output[11];
uint16_t SITL_State::pwm_output[11];
uint16_t SITL_State::pwm_input[8];
// catch floating point exceptions
void SITL_State::_sig_fpe(int signum)
@ -99,7 +97,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_framerate = 50;
}
// set right default throttle for rover (allowing for reverse)
ICR4.set(2, 1500);
pwm_input[2] = 1500;
} else {
_vehicle = ArduPlane;
if (_framerate == 0) {
@ -302,10 +300,9 @@ void SITL_State::_fdm_input(void)
// a packet giving the receiver PWM inputs
uint8_t i;
for (i=0; i<8; i++) {
// setup the ICR4 register for the RC channel
// inputs
// setup the pwn input for the RC channel inputs
if (d.pwm_pkt.pwm[i] != 0) {
ICR4.set(i, d.pwm_pkt.pwm[i]);
pwm_input[i] = d.pwm_pkt.pwm[i];
}
}
break;
@ -331,15 +328,15 @@ void SITL_State::_simulator_output(void)
if (last_update == 0) {
for (i=0; i<11; i++) {
_pwm_output[i] = 1000;
pwm_output[i] = 1000;
}
if (_vehicle == ArduPlane) {
_pwm_output[0] = _pwm_output[1] = _pwm_output[3] = 1500;
_pwm_output[7] = 1800;
pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500;
pwm_output[7] = 1800;
}
if (_vehicle == APMrover2) {
_pwm_output[0] = _pwm_output[1] = _pwm_output[2] = _pwm_output[3] = 1500;
_pwm_output[7] = 1800;
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500;
pwm_output[7] = 1800;
}
}
@ -354,10 +351,10 @@ void SITL_State::_simulator_output(void)
last_update = hal.scheduler->millis();
for (i=0; i<11; i++) {
if (_pwm_output[i] == 0xFFFF) {
if (pwm_output[i] == 0xFFFF) {
control.pwm[i] = 0;
} else {
control.pwm[i] = _pwm_output[i];
control.pwm[i] = pwm_output[i];
}
}
@ -445,6 +442,10 @@ Vector3f SITL_State::_rand_vec3f(void)
void SITL_State::init(int argc, char * const argv[])
{
pwm_input[0] = pwm_input[1] = pwm_input[3] = 1500;
pwm_input[4] = pwm_input[7] = 1800;
pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000;
_scheduler = (SITLScheduler *)hal.scheduler;
_parse_command_line(argc, argv);
}

7
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -9,7 +9,6 @@
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include "sitl_rc.h"
#include <sys/types.h>
#include <sys/socket.h>
@ -22,8 +21,6 @@
#include "../AP_Compass/AP_Compass.h"
#include "../SITL/SITL.h"
extern struct RC_ICR4 ICR4;
class HAL_AVR_SITL;
class AVR_SITL::SITL_State {
@ -38,6 +35,8 @@ public:
int gps_pipe(void);
ssize_t gps_read(int fd, void *buf, size_t count);
static uint16_t pwm_output[11];
static uint16_t pwm_input[8];
private:
void _parse_command_line(int argc, char * const argv[]);
@ -88,8 +87,6 @@ private:
static SITL *_sitl;
static const uint16_t _rcout_port = 5502;
static const uint16_t _simin_port = 5501;
static uint16_t _pwm_output[11];
};
#endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

52
libraries/AP_HAL_AVR_SITL/sitl_rc.h
View File

@ -1,52 +0,0 @@
/*
RC input emulation
Code by Andrew Tridgell November 2011
*/
#ifndef _SITL_RC_H
#define _SITL_RC_H
struct RC_ICR4 {
uint16_t channels[9]; // 9th channel is sync
uint32_t value;
uint8_t idx;
RC_ICR4() {
// constructor
channels[0] = channels[1] = channels[3] = 1500;
channels[4] = channels[7] = 1800;
channels[2] = channels[5] = channels[6] = 1000;
channels[8] = 4500; // sync
idx = 0;
}
/*
read from ICR4 fetches next channel
*/
operator int() {
value += channels[idx++]*2;
if (idx == 9) {
idx = 0;
}
value = value % 40000;
return (uint16_t)value;
}
/*
ignore rate assignment for now (needed for apm2
emulation)
*/
RC_ICR4& operator=(uint16_t rate) {
return *this;
}
/*
interface to set a channel value from SITL
*/
void set(uint8_t i, uint16_t v) {
channels[i] = v;
}
};
#endif