mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
3716f5513d
this is the loop which ensures the amount of data sent to the mavlink client (usually Python) is limited - if we don't do this then we lose vast amounts of data when running at large speedups. By attempting to process inbound data we may realise that the TCP connection has been dropped, and in that case we will start to listen for another connection. This allows you to terminate the sim_vehicle.py MAVProxy and have it automagically restart (when running under GDB). This is very useful for testing MAVProxy patches with SITL; it's a different workflow to opening an output and connecting a new version of MAVProxy to that outout.
631 lines
19 KiB
C++
631 lines
19 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
|
|
|
|
#include "AP_HAL_SITL.h"
|
|
#include "AP_HAL_SITL_Namespace.h"
|
|
#include "HAL_SITL_Class.h"
|
|
#include "UARTDriver.h"
|
|
#include "Scheduler.h"
|
|
#include "CANSocketIface.h"
|
|
|
|
#include <stdio.h>
|
|
#include <signal.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <errno.h>
|
|
#include <sys/types.h>
|
|
#include <sys/select.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
#include <fcntl.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <SITL/SIM_JSBSim.h>
|
|
#include <AP_HAL/utility/Socket_native.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
using namespace HALSITL;
|
|
|
|
void SITL_State::_set_param_default(const char *parm)
|
|
{
|
|
char *pdup = strdup(parm);
|
|
char *p = strchr(pdup, '=');
|
|
if (p == nullptr) {
|
|
printf("Please specify parameter as NAME=VALUE");
|
|
exit(1);
|
|
}
|
|
float value = strtof(p+1, nullptr);
|
|
*p = 0;
|
|
enum ap_var_type var_type;
|
|
AP_Param *vp = AP_Param::find(pdup, &var_type);
|
|
if (vp == nullptr) {
|
|
printf("Unknown parameter %s\n", pdup);
|
|
exit(1);
|
|
}
|
|
if (var_type == AP_PARAM_FLOAT) {
|
|
((AP_Float *)vp)->set_and_save(value);
|
|
} else if (var_type == AP_PARAM_INT32) {
|
|
((AP_Int32 *)vp)->set_and_save(value);
|
|
} else if (var_type == AP_PARAM_INT16) {
|
|
((AP_Int16 *)vp)->set_and_save(value);
|
|
} else if (var_type == AP_PARAM_INT8) {
|
|
((AP_Int8 *)vp)->set_and_save(value);
|
|
} else {
|
|
printf("Unable to set parameter %s\n", pdup);
|
|
exit(1);
|
|
}
|
|
printf("Set parameter %s to %f\n", pdup, value);
|
|
free(pdup);
|
|
}
|
|
|
|
|
|
/*
|
|
setup for SITL handling
|
|
*/
|
|
void SITL_State::_sitl_setup()
|
|
{
|
|
#if !defined(__CYGWIN__) && !defined(__CYGWIN64__)
|
|
_parent_pid = getppid();
|
|
#endif
|
|
|
|
fprintf(stdout, "Starting SITL input\n");
|
|
|
|
// find the barometer object if it exists
|
|
_sitl = AP::sitl();
|
|
|
|
if (_sitl != nullptr) {
|
|
// setup some initial values
|
|
_update_airspeed(0);
|
|
#if AP_SIM_SOLOGIMBAL_ENABLED
|
|
if (enable_gimbal) {
|
|
gimbal = NEW_NOTHROW SITL::SoloGimbal();
|
|
}
|
|
#endif
|
|
|
|
sitl_model->set_buzzer(&_sitl->buzzer_sim);
|
|
sitl_model->set_sprayer(&_sitl->sprayer_sim);
|
|
sitl_model->set_gripper_servo(&_sitl->gripper_sim);
|
|
sitl_model->set_gripper_epm(&_sitl->gripper_epm_sim);
|
|
sitl_model->set_parachute(&_sitl->parachute_sim);
|
|
sitl_model->set_precland(&_sitl->precland_sim);
|
|
_sitl->i2c_sim.init();
|
|
sitl_model->set_i2c(&_sitl->i2c_sim);
|
|
#if AP_TEST_DRONECAN_DRIVERS
|
|
sitl_model->set_dronecan_device(&_sitl->dronecan_sim);
|
|
#endif
|
|
if (_use_fg_view) {
|
|
fg_socket.connect(_fg_address, _fg_view_port);
|
|
}
|
|
|
|
fprintf(stdout, "Using Irlock at port : %d\n", _irlock_port);
|
|
_sitl->irlock_port = _irlock_port;
|
|
|
|
_sitl->rcin_port = _rcin_port;
|
|
}
|
|
|
|
if (_synthetic_clock_mode) {
|
|
// start with non-zero clock
|
|
hal.scheduler->stop_clock(1);
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
step the FDM by one time step
|
|
*/
|
|
void SITL_State::_fdm_input_step(void)
|
|
{
|
|
_fdm_input_local();
|
|
|
|
/* make sure we die if our parent dies */
|
|
if (kill(_parent_pid, 0) != 0) {
|
|
exit(1);
|
|
}
|
|
|
|
if (_scheduler->interrupts_are_blocked() || _sitl == nullptr) {
|
|
return;
|
|
}
|
|
|
|
_scheduler->sitl_begin_atomic();
|
|
|
|
if (_update_count == 0 && _sitl != nullptr) {
|
|
HALSITL::Scheduler::timer_event();
|
|
_scheduler->sitl_end_atomic();
|
|
return;
|
|
}
|
|
|
|
if (_sitl != nullptr) {
|
|
_update_airspeed(_sitl->state.airspeed);
|
|
_update_rangefinder();
|
|
}
|
|
|
|
// trigger all APM timers.
|
|
HALSITL::Scheduler::timer_event();
|
|
_scheduler->sitl_end_atomic();
|
|
}
|
|
|
|
|
|
void SITL_State::wait_clock(uint64_t wait_time_usec)
|
|
{
|
|
float speedup = sitl_model->get_speedup();
|
|
if (speedup < 1) {
|
|
// for purposes of sleeps treat low speedups as 1
|
|
speedup = 1.0;
|
|
}
|
|
while (AP_HAL::micros64() < wait_time_usec) {
|
|
if (hal.scheduler->in_main_thread() ||
|
|
Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
|
|
_fdm_input_step();
|
|
} else {
|
|
#ifdef CYGWIN_BUILD
|
|
if (speedup > 2 && hal.util->get_soft_armed()) {
|
|
const char *current_thread = Scheduler::from(hal.scheduler)->get_current_thread_name();
|
|
if (current_thread && strcmp(current_thread, "Scripting") == 0) {
|
|
// this effectively does a yield of the CPU. The
|
|
// granularity of sleeps on cygwin is very high,
|
|
// so this is needed for good thread performance
|
|
// in scripting. We don't do this at low speedups
|
|
// as it causes the cpu to run hot
|
|
// We also don't do it while disarmed, as lua performance is less
|
|
// critical while disarmed
|
|
usleep(0);
|
|
continue;
|
|
}
|
|
}
|
|
#endif
|
|
usleep(1000);
|
|
}
|
|
}
|
|
// check the outbound TCP queue size. If it is too long then
|
|
// MAVProxy/pymavlink take too long to process packets and it ends
|
|
// up seeing traffic well into our past and hits time-out
|
|
// conditions.
|
|
if (speedup > 1 && hal.scheduler->in_main_thread()) {
|
|
while (true) {
|
|
HALSITL::UARTDriver *uart = (HALSITL::UARTDriver*)hal.serial(0);
|
|
const int queue_length = uart->get_system_outqueue_length();
|
|
// ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length);
|
|
if (queue_length < 1024) {
|
|
break;
|
|
}
|
|
_serial_0_outqueue_full_count++;
|
|
uart->handle_reading_from_device_to_readbuffer();
|
|
usleep(1000);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
output current state to flightgear
|
|
*/
|
|
void SITL_State::_output_to_flightgear(void)
|
|
{
|
|
SITL::FGNetFDM fdm {};
|
|
const SITL::sitl_fdm &sfdm = _sitl->state;
|
|
|
|
fdm.version = 0x18;
|
|
fdm.padding = 0;
|
|
fdm.longitude = DEG_TO_RAD_DOUBLE*sfdm.longitude;
|
|
fdm.latitude = DEG_TO_RAD_DOUBLE*sfdm.latitude;
|
|
fdm.altitude = sfdm.altitude;
|
|
fdm.agl = sfdm.altitude;
|
|
fdm.phi = radians(sfdm.rollDeg);
|
|
fdm.theta = radians(sfdm.pitchDeg);
|
|
fdm.psi = radians(sfdm.yawDeg);
|
|
fdm.vcas = sfdm.velocity_air_bf.length()/0.3048;
|
|
if (_vehicle == ArduCopter) {
|
|
fdm.num_engines = 4;
|
|
for (uint8_t i=0; i<4; i++) {
|
|
fdm.rpm[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
|
|
}
|
|
} else {
|
|
fdm.num_engines = 4;
|
|
fdm.rpm[0] = constrain_float((pwm_output[2]-1000)*3, 0, 3000);
|
|
// for quadplane
|
|
fdm.rpm[1] = constrain_float((pwm_output[5]-1000)*12, 0, 12000);
|
|
fdm.rpm[2] = constrain_float((pwm_output[6]-1000)*12, 0, 12000);
|
|
fdm.rpm[3] = constrain_float((pwm_output[7]-1000)*12, 0, 12000);
|
|
}
|
|
fdm.ByteSwap();
|
|
|
|
fg_socket.send(&fdm, sizeof(fdm));
|
|
}
|
|
|
|
/*
|
|
get FDM input from a local model
|
|
*/
|
|
void SITL_State::_fdm_input_local(void)
|
|
{
|
|
if (_sitl == nullptr) {
|
|
return;
|
|
}
|
|
struct sitl_input input;
|
|
|
|
// construct servos structure for FDM
|
|
_simulator_servos(input);
|
|
|
|
#if HAL_SIM_JSON_MASTER_ENABLED
|
|
// read servo inputs from ride along flight controllers
|
|
ride_along.receive(input);
|
|
#endif
|
|
|
|
// replace outputs from multicast
|
|
multicast_servo_update(input);
|
|
|
|
// update the model
|
|
sitl_model->update_home();
|
|
sitl_model->update_model(input);
|
|
|
|
// get FDM output from the model
|
|
sitl_model->fill_fdm(_sitl->state);
|
|
|
|
#if HAL_NUM_CAN_IFACES
|
|
if (CANIface::num_interfaces() > 0) {
|
|
multicast_state_send();
|
|
}
|
|
#endif
|
|
|
|
#if HAL_SIM_JSON_MASTER_ENABLED
|
|
// output JSON state to ride along flight controllers
|
|
ride_along.send(_sitl->state,sitl_model->get_position_relhome());
|
|
#endif
|
|
|
|
sim_update();
|
|
|
|
if (_use_fg_view) {
|
|
_output_to_flightgear();
|
|
}
|
|
|
|
// update simulation time
|
|
hal.scheduler->stop_clock(_sitl->state.timestamp_us);
|
|
|
|
set_height_agl();
|
|
|
|
_synthetic_clock_mode = true;
|
|
_update_count++;
|
|
}
|
|
|
|
/*
|
|
create sitl_input structure for sending to FDM
|
|
*/
|
|
void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
{
|
|
if (_sitl == nullptr) {
|
|
return;
|
|
}
|
|
static uint32_t last_update_usec;
|
|
|
|
/* this maps the registers used for PWM outputs. The RC
|
|
* driver updates these whenever it wants the channel output
|
|
* to change */
|
|
|
|
if (last_update_usec == 0 || !output_ready) {
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
|
|
pwm_output[i] = 1000;
|
|
}
|
|
if (_vehicle == ArduPlane) {
|
|
pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500;
|
|
}
|
|
if (_vehicle == Rover) {
|
|
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500;
|
|
}
|
|
if (_vehicle == ArduSub) {
|
|
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] =
|
|
pwm_output[4] = pwm_output[5] = pwm_output[6] = pwm_output[7] = 1500;
|
|
}
|
|
}
|
|
|
|
// output at chosen framerate
|
|
uint32_t now = AP_HAL::micros();
|
|
last_update_usec = now;
|
|
|
|
float altitude = AP::baro().get_altitude();
|
|
float wind_speed = 0;
|
|
float wind_direction = 0;
|
|
float wind_dir_z = 0;
|
|
|
|
// give 5 seconds to calibrate airspeed sensor at 0 wind speed
|
|
if (wind_start_delay_micros == 0) {
|
|
wind_start_delay_micros = now;
|
|
} else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) {
|
|
// The EKF does not like step inputs so this LPF keeps it happy.
|
|
uint32_t dt_us = now - last_wind_update_us;
|
|
if (dt_us > 1000) {
|
|
last_wind_update_us = now;
|
|
// slew wind based on the configured time constant
|
|
const float dt = dt_us * 1.0e-6;
|
|
const float tc = MAX(_sitl->wind_change_tc, 0.1);
|
|
const float alpha = calc_lowpass_alpha_dt(dt, 1.0/tc);
|
|
_sitl->wind_speed_active += (_sitl->wind_speed - _sitl->wind_speed_active) * alpha;
|
|
_sitl->wind_direction_active += (wrap_180(_sitl->wind_direction - _sitl->wind_direction_active)) * alpha;
|
|
_sitl->wind_dir_z_active += (_sitl->wind_dir_z - _sitl->wind_dir_z_active) * alpha;
|
|
_sitl->wind_direction_active = wrap_180(_sitl->wind_direction_active);
|
|
}
|
|
wind_speed = _sitl->wind_speed_active;
|
|
wind_direction = _sitl->wind_direction_active;
|
|
wind_dir_z = _sitl->wind_dir_z_active;
|
|
|
|
// pass wind into simulators using different wind types via param SIM_WIND_T*.
|
|
switch (_sitl->wind_type) {
|
|
case SITL::SIM::WIND_TYPE_SQRT:
|
|
if (altitude < _sitl->wind_type_alt) {
|
|
wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0));
|
|
}
|
|
break;
|
|
|
|
case SITL::SIM::WIND_TYPE_COEF:
|
|
wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef;
|
|
break;
|
|
|
|
case SITL::SIM::WIND_TYPE_NO_LIMIT:
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// never allow negative wind velocity
|
|
wind_speed = MAX(wind_speed, 0);
|
|
}
|
|
|
|
input.wind.speed = wind_speed;
|
|
input.wind.direction = wind_direction;
|
|
input.wind.turbulence = _sitl?_sitl->wind_turbulance:0;
|
|
input.wind.dir_z = wind_dir_z;
|
|
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
|
|
if (pwm_output[i] == 0xFFFF) {
|
|
input.servos[i] = 0;
|
|
} else {
|
|
input.servos[i] = pwm_output[i];
|
|
}
|
|
}
|
|
|
|
if (_sitl != nullptr) {
|
|
// FETtec ESC simulation support. Input signals of 1000-2000
|
|
// are positive thrust, 0 to 1000 are negative thrust. Deeper
|
|
// changes required to support negative thrust - potentially
|
|
// adding a field to input.
|
|
if (_sitl != nullptr) {
|
|
if (_sitl->fetteconewireesc_sim.enabled()) {
|
|
_sitl->fetteconewireesc_sim.update_sitl_input_pwm(input);
|
|
for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
|
|
if (input.servos[i] != 0 && input.servos[i] < 1000) {
|
|
AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
float engine_mul = _sitl?_sitl->engine_mul.get():1;
|
|
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
|
|
float throttle = 0.0f;
|
|
|
|
if (engine_fail >= ARRAY_SIZE(input.servos)) {
|
|
engine_fail = 0;
|
|
}
|
|
// apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
|
|
if (_vehicle != Rover) {
|
|
input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000;
|
|
} else {
|
|
input.servos[engine_fail] = static_cast<uint16_t>(((input.servos[engine_fail] - 1500) * engine_mul) + 1500);
|
|
}
|
|
|
|
if (_vehicle == ArduPlane) {
|
|
float forward_throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
|
|
// do a little quadplane dance
|
|
float hover_throttle = 0.0f;
|
|
uint8_t running_motors = 0;
|
|
uint32_t mask = _sitl->state.motor_mask;
|
|
uint8_t bit;
|
|
while ((bit = __builtin_ffs(mask)) != 0) {
|
|
uint8_t motor = bit-1;
|
|
mask &= ~(1U<<motor);
|
|
float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
|
|
// update motor_on flag
|
|
if (!is_zero(motor_throttle)) {
|
|
hover_throttle += motor_throttle;
|
|
running_motors++;
|
|
}
|
|
}
|
|
if (running_motors > 0) {
|
|
hover_throttle /= running_motors;
|
|
}
|
|
if (!is_zero(forward_throttle)) {
|
|
throttle = forward_throttle;
|
|
} else {
|
|
throttle = hover_throttle;
|
|
}
|
|
} else if (_vehicle == Rover) {
|
|
input.servos[2] = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000));
|
|
input.servos[0] = static_cast<uint16_t>(constrain_int16(input.servos[0], 1000, 2000));
|
|
throttle = fabsf((input.servos[2] - 1500) / 500.0f);
|
|
} else {
|
|
// run checks on each motor
|
|
uint8_t running_motors = 0;
|
|
uint32_t mask = _sitl->state.motor_mask;
|
|
uint8_t bit;
|
|
while ((bit = __builtin_ffs(mask)) != 0) {
|
|
const uint8_t motor = bit-1;
|
|
mask &= ~(1U<<motor);
|
|
float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
|
|
// update motor_on flag
|
|
if (!is_zero(motor_throttle)) {
|
|
throttle += motor_throttle;
|
|
running_motors++;
|
|
}
|
|
}
|
|
if (running_motors > 0) {
|
|
throttle /= running_motors;
|
|
}
|
|
}
|
|
if (_sitl) {
|
|
_sitl->throttle = throttle;
|
|
}
|
|
|
|
update_voltage_current(input, throttle);
|
|
}
|
|
|
|
void SITL_State::init(int argc, char * const argv[])
|
|
{
|
|
_scheduler = Scheduler::from(hal.scheduler);
|
|
_parse_command_line(argc, argv);
|
|
}
|
|
|
|
/*
|
|
set height above the ground in meters
|
|
*/
|
|
void SITL_State::set_height_agl(void)
|
|
{
|
|
static float home_alt = -1;
|
|
|
|
if (!_sitl) {
|
|
// in example program
|
|
return;
|
|
}
|
|
|
|
if (is_equal(home_alt, -1.0f) && _sitl->state.altitude > 0) {
|
|
// remember home altitude as first non-zero altitude
|
|
home_alt = _sitl->state.altitude;
|
|
}
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
if (_sitl != nullptr &&
|
|
_sitl->terrain_enable) {
|
|
// get height above terrain from AP_Terrain. This assumes
|
|
// AP_Terrain is working
|
|
float terrain_height_amsl;
|
|
Location location;
|
|
location.lat = _sitl->state.latitude*1.0e7;
|
|
location.lng = _sitl->state.longitude*1.0e7;
|
|
|
|
AP_Terrain *_terrain = AP_Terrain::get_singleton();
|
|
if (_terrain != nullptr &&
|
|
_terrain->height_amsl(location, terrain_height_amsl, false)) {
|
|
_sitl->state.height_agl = _sitl->state.altitude - terrain_height_amsl;
|
|
return;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
if (_sitl != nullptr) {
|
|
// fall back to flat earth model
|
|
_sitl->state.height_agl = _sitl->state.altitude - home_alt;
|
|
}
|
|
}
|
|
|
|
/*
|
|
open multicast UDP
|
|
*/
|
|
void SITL_State::multicast_state_open(void)
|
|
{
|
|
struct sockaddr_in sockaddr {};
|
|
int ret;
|
|
|
|
#ifdef HAVE_SOCK_SIN_LEN
|
|
sockaddr.sin_len = sizeof(sockaddr);
|
|
#endif
|
|
sockaddr.sin_port = htons(SITL_MCAST_PORT);
|
|
sockaddr.sin_family = AF_INET;
|
|
sockaddr.sin_addr.s_addr = inet_addr(SITL_MCAST_IP);
|
|
|
|
mc_out_fd = socket(AF_INET, SOCK_DGRAM, 0);
|
|
if (mc_out_fd == -1) {
|
|
fprintf(stderr, "socket failed - %s\n", strerror(errno));
|
|
exit(1);
|
|
}
|
|
ret = fcntl(mc_out_fd, F_SETFD, FD_CLOEXEC);
|
|
if (ret == -1) {
|
|
fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
|
|
exit(1);
|
|
}
|
|
|
|
// try to setup for broadcast, this may fail if insufficient privileges
|
|
int one = 1;
|
|
setsockopt(mc_out_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
|
|
|
|
ret = connect(mc_out_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
|
|
if (ret == -1) {
|
|
fprintf(stderr, "udp connect failed on port %u - %s\n",
|
|
(unsigned)ntohs(sockaddr.sin_port),
|
|
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 + _instance);
|
|
|
|
ret = bind(servo_in_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
|
|
if (ret == -1) {
|
|
fprintf(stderr, "udp servo connect failed\n");
|
|
exit(1);
|
|
}
|
|
::printf("multicast initialised\n");
|
|
}
|
|
|
|
/*
|
|
send out SITL state as multicast UDP
|
|
*/
|
|
void SITL_State::multicast_state_send(void)
|
|
{
|
|
if (_sitl == nullptr) {
|
|
return;
|
|
}
|
|
if (mc_out_fd == -1) {
|
|
multicast_state_open();
|
|
}
|
|
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
|
|
float mc_servo_float[SITL_NUM_CHANNELS];
|
|
// we loop to ensure we drain all packets from all nodes
|
|
while (recv(servo_in_fd, (void*)mc_servo_float, sizeof(mc_servo_float), MSG_DONTWAIT) == sizeof(mc_servo_float)) {
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
|
|
// nan means that node is not outputting this channel
|
|
if (!isnan(mc_servo_float[i])) {
|
|
mc_servo[i] = uint16_t(mc_servo_float[i]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
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);
|
|
const uint32_t can_mask = uint32_t(_sitl->can_servo_mask.get());
|
|
if (can_mask & mask) {
|
|
input.servos[i] = mc_servo[i];
|
|
}
|
|
}
|
|
}
|
|
#endif
|