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

HAL_SITL: support servo and ESC output in SITL AP_Periph 

use another UDP socket back to the main firmware from peripheral
This commit is contained in:
Andrew Tridgell 2023-08-19 16:09:50 +10:00 committed by Peter Barker
parent 51af21f6d9
commit 1144036a8d
7 changed files with 121 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL_SITL/RCOutput.cpp
View File

@ -1,7 +1,5 @@
#include <AP_HAL/AP_HAL.h>
#if !defined(HAL_BUILD_AP_PERIPH)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_BoardConfig/AP_BoardConfig.h>
@ -157,5 +155,3 @@ void RCOutput::serial_led_send(const uint16_t chan)
}
#endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL
#endif //!defined(HAL_BUILD_AP_PERIPH)

2
libraries/AP_HAL_SITL/RCOutput.h
View File

@ -1,7 +1,7 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "AP_HAL_SITL.h"
#include <AP_ESC_Telem/AP_ESC_Telem_SITL.h>

51
libraries/AP_HAL_SITL/SITL_Periph_State.cpp
View File

@ -161,9 +161,6 @@ void SimMCast::multicast_open(void)
exit(1);
}
// close on exec, to allow reboot
fcntl(mc_fd, F_SETFD, FD_CLOEXEC);
#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
/*
on cygwin you need to bind to INADDR_ANY then use the multicast
@ -193,6 +190,46 @@ void SimMCast::multicast_open(void)
}
}
/*
open UDP socket back to master for servo output
*/
void SimMCast::servo_fd_open(void)
{
servo_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (servo_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
int ret = fcntl(servo_fd, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
int one = 1;
if (setsockopt(servo_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
fprintf(stderr, "setsockopt failed: %s\n", strerror(errno));
exit(1);
}
in_addr.sin_port = htons(SITL_SERVO_PORT);
ret = connect(servo_fd, (struct sockaddr *)&in_addr, sizeof(in_addr));
if (ret == -1) {
fprintf(stderr, "multicast servo connect failed\n");
exit(1);
}
}
/*
send servo outputs back to master
*/
void SimMCast::servo_send(void)
{
uint16_t out[SITL_NUM_CHANNELS] {};
hal.rcout->read(out, SITL_NUM_CHANNELS);
send(servo_fd, (void*)out, sizeof(out), 0);
}
/*
read state from multicast
*/
@ -206,7 +243,8 @@ void SimMCast::multicast_read(void)
printf("Waiting for multicast state\n");
}
struct SITL::sitl_fdm state;
if (recv(mc_fd, (void*)&state, sizeof(state), MSG_WAITALL) == sizeof(state)) {
socklen_t len = sizeof(in_addr);
if (recvfrom(mc_fd, (void*)&state, sizeof(state), MSG_WAITALL, (sockaddr *)&in_addr, &len) == sizeof(state)) {
if (state.timestamp_us < _sitl->state.timestamp_us) {
// main process has rebooted
base_time_us += (_sitl->state.timestamp_us - state.timestamp_us);
@ -214,6 +252,11 @@ void SimMCast::multicast_read(void)
_sitl->state = state;
hal.scheduler->stop_clock(_sitl->state.timestamp_us + base_time_us);
HALSITL::Scheduler::timer_event();
if (servo_fd == -1) {
servo_fd_open();
} else {
servo_send();
}
}
}

7
libraries/AP_HAL_SITL/SITL_Periph_State.h
View File

@ -30,13 +30,18 @@ public:
void update(const struct sitl_input &input) override;
private:
int mc_fd;
int mc_fd = -1;
int servo_fd = -1;
struct sockaddr_in in_addr;
// offset between multicast timestamp and local timestamp
uint64_t base_time_us;
void multicast_open();
void multicast_read();
void servo_send(void);
void servo_fd_open(void);
};
class HAL_SITL;

56
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -342,6 +342,9 @@ void SITL_State::_fdm_input_local(void)
ride_along.receive(input);
#endif
// replace outputs from multicast
multicast_servo_update(input);
// update the model
sitl_model->update_model(input);
@ -640,6 +643,29 @@ void SITL_State::multicast_state_open(void)
strerror(errno));
exit(1);
}
/*
open servo input socket
*/
servo_in_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (servo_in_fd == -1) {
fprintf(stderr, "socket failed - %s\n", strerror(errno));
exit(1);
}
ret = fcntl(servo_in_fd, F_SETFD, FD_CLOEXEC);
if (ret == -1) {
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
exit(1);
}
sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
sockaddr.sin_port = htons(SITL_SERVO_PORT);
ret = bind(servo_in_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
fprintf(stderr, "udp servo connect failed\n");
exit(1);
}
}
/*
@ -655,6 +681,36 @@ void SITL_State::multicast_state_send(void)
}
const auto &sfdm = _sitl->state;
send(mc_out_fd, (void*)&sfdm, sizeof(sfdm), 0);
check_servo_input();
}
/*
check for servo data from peripheral
*/
void SITL_State::check_servo_input(void)
{
// drain any pending packets
while (recv(servo_in_fd, (void*)mc_servo, sizeof(mc_servo), MSG_DONTWAIT) == sizeof(mc_servo)) {
// noop
}
}
/*
overwrite input structure with multicast values
*/
void SITL_State::multicast_servo_update(struct sitl_input &input)
{
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
const uint32_t mask = (1U<<i);
if (mc_servo[i] != 0) {
// we consider an output active if it has ever seen
// a non-zero value
servo_active_mask |= mask;
}
if (servo_active_mask & mask) {
input.servos[i] = mc_servo[i];
}
}
}
#endif

10
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -19,9 +19,7 @@ class HALSITL::SITL_State : public SITL_State_Common {
public:
void init(int argc, char * const argv[]);
uint16_t pwm_output[SITL_NUM_CHANNELS];
uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
bool output_ready = false;
bool new_rc_input;
void loop_hook(void);
uint16_t base_port(void) const {
@ -125,10 +123,16 @@ private:
// multicast state
int mc_out_fd = -1;
int servo_in_fd = -1;
// send out SITL state as UDP multicast
void multicast_state_open(void);
void multicast_state_send(void);
void multicast_servo_update(struct sitl_input &input);
uint32_t servo_active_mask;
uint16_t mc_servo[SITL_NUM_CHANNELS];
void check_servo_input(void);
};
#endif // defined(HAL_BUILD_AP_PERIPH)

4
libraries/AP_HAL_SITL/SITL_State_common.h
View File

@ -6,6 +6,7 @@
#define SITL_MCAST_IP "239.255.145.51"
#define SITL_MCAST_PORT 20721
#define SITL_SERVO_PORT 20722
#include <SITL/SIM_Gimbal.h>
#include <SITL/SIM_ADSB.h>
@ -95,6 +96,9 @@ public:
float voltage2_pin_voltage; // pin 15
float current2_pin_voltage; // pin 14
uint16_t pwm_output[SITL_NUM_CHANNELS];
bool output_ready = false;
#if HAL_SIM_GIMBAL_ENABLED
// simulated gimbal
bool enable_gimbal;