Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: support 16 RC input channels in SITL

This commit is contained in:
Andrew Tridgell 2016-01-04 08:22:02 +11:00
parent 0966398d8d
commit 1450f33b51
4 changed files with 43 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_HAL_SITL/RCInput.cpp
View File

@ -21,31 +21,41 @@ bool SITLRCInput::new_input()
return false;
}
uint16_t SITLRCInput::read(uint8_t ch) {
if (ch >= 8) {
uint16_t SITLRCInput::read(uint8_t ch)
{
if (ch >= SITL_RC_INPUT_CHANNELS) {
return 0;
}
return _override[ch]? _override[ch] : _sitlState->pwm_input[ch];
}
uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) {
uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len)
{
if (len > SITL_RC_INPUT_CHANNELS) {
len = SITL_RC_INPUT_CHANNELS;
}
for (uint8_t i=0; i<len; i++) {
periods[i] = _override[i]? _override[i] : _sitlState->pwm_input[i];
}
return 8;
}
bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len) {
bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len)
{
bool res = false;
if (len > SITL_RC_INPUT_CHANNELS) {
len = SITL_RC_INPUT_CHANNELS;
}
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) {
bool SITLRCInput::set_override(uint8_t channel, int16_t override)
{
if (override < 0) return false; /* -1: no change. */
if (channel < 8) {
if (channel < SITL_RC_INPUT_CHANNELS) {
_override[channel] = override;
if (override != 0) {
return true;
@ -56,8 +66,6 @@ bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
void SITLRCInput::clear_overrides()
{
for (uint8_t i = 0; i < 8; i++) {
_override[i] = 0;
}
memset(_override, 0, sizeof(_override));
}
#endif

26
libraries/AP_HAL_SITL/RCInput.h
View File

@ -1,9 +1,10 @@
#ifndef __AP_HAL_SITL_RCINPUT_H__
#define __AP_HAL_SITL_RCINPUT_H__
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define SITL_RC_INPUT_CHANNELS 16
#include "AP_HAL_SITL.h"
class HALSITL::SITLRCInput : public AP_HAL::RCInput {
@ -11,26 +12,25 @@ public:
SITLRCInput(SITL_State *sitlState) {
_sitlState = sitlState;
}
void init();
bool new_input();
uint8_t num_channels() {
return 8;
void init() override;
bool new_input() override;
uint8_t num_channels() override {
return SITL_RC_INPUT_CHANNELS;
}
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
uint16_t read(uint8_t ch) override;
uint8_t read(uint16_t* periods, uint8_t len) override;
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
bool set_overrides(int16_t *overrides, uint8_t len) override;
bool set_override(uint8_t channel, int16_t override) override;
void clear_overrides() override;
private:
SITL_State *_sitlState;
bool _valid;
/* override state */
uint16_t _override[8];
uint16_t _override[SITL_RC_INPUT_CHANNELS];
};
#endif
#endif // __AP_HAL_SITL_RCINPUT_H__

40
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -19,6 +19,7 @@
#include <AP_Param/AP_Param.h>
#include <SITL/SIM_JSBSim.h>
#include <AP_HAL/utility/Socket.h>
extern const AP_HAL::HAL& hal;
@ -114,34 +115,12 @@ void SITL_State::_sitl_setup(const char *home_str)
*/
void SITL_State::_setup_fdm(void)
{
int one=1, ret;
struct sockaddr_in sockaddr;
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(_simin_port);
sockaddr.sin_family = AF_INET;
_sitl_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (_sitl_fd == -1) {
fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno));
if (!_sitl_rc_in.bind("0.0.0.0", _simin_port)) {
fprintf(stderr, "SITL: socket bind failed - %s\n", strerror(errno));
exit(1);
}
/* we want to be able to re-use ports quickly */
setsockopt(_sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
ret = bind(_sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "SITL: bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port), strerror(errno));
exit(1);
}
HALSITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
_sitl_rc_in.reuseaddress();
_sitl_rc_in.set_blocking(false);
}
#endif
@ -215,15 +194,16 @@ void SITL_State::_fdm_input(void)
{
ssize_t size;
struct pwm_packet {
uint16_t pwm[8];
uint16_t pwm[16];
} pwm_pkt;
size = recv(_sitl_fd, &pwm_pkt, sizeof(pwm_pkt), MSG_DONTWAIT);
size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
switch (size) {
case sizeof(pwm_pkt): {
case 8*2:
case 16*2: {
// a packet giving the receiver PWM inputs
uint8_t i;
for (i=0; i<8; i++) {
for (i=0; i<size/2; i++) {
// setup the pwn input for the RC channel inputs
if (pwm_pkt.pwm[i] != 0) {
pwm_input[i] = pwm_pkt.pwm[i];

5
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -9,6 +9,7 @@
#include "AP_HAL_SITL.h"
#include "AP_HAL_SITL_Namespace.h"
#include "HAL_SITL_Class.h"
#include "RCInput.h"
#include <sys/types.h>
#include <sys/socket.h>
@ -44,7 +45,7 @@ public:
ssize_t gps_read(int fd, void *buf, size_t count);
uint16_t pwm_output[SITL_NUM_CHANNELS];
uint16_t last_pwm_output[SITL_NUM_CHANNELS];
uint16_t pwm_input[8];
uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
bool new_rc_input;
void loop_hook(void);
uint16_t base_port(void) const {
@ -148,7 +149,7 @@ private:
OpticalFlow *_optical_flow;
AP_Terrain *_terrain;
int _sitl_fd;
SocketAPM _sitl_rc_in{true};
SITL::SITL *_sitl;
uint16_t _rcout_port;
uint16_t _simin_port;