mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
691 lines
22 KiB
C++
691 lines
22 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 "SITL_Multicast.h"
|
|
|
|
#include <stdio.h>
|
|
#include <signal.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <string.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 <poll.h>
|
|
#include <time.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <SITL/SIM_JSBSim.h>
|
|
#include <AP_HAL/utility/Socket_native.h>
|
|
|
|
#include <AP_HAL/SIMState.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
using namespace HALSITL;
|
|
|
|
/*
|
|
setup for SITL handling
|
|
*/
|
|
void SITL_State::_sitl_setup()
|
|
{
|
|
#if !defined(__CYGWIN__) && !defined(__CYGWIN64__)
|
|
_parent_pid = getppid();
|
|
#endif
|
|
|
|
fprintf(stdout, "Starting SITL input\n");
|
|
|
|
_sitl = AP::sitl();
|
|
|
|
if (_sitl != nullptr) {
|
|
// setup some initial values
|
|
_update_airspeed(0);
|
|
#if AP_SIM_SOLOGIMBAL_ENABLED
|
|
if (enable_gimbal) {
|
|
// the gimbal connects back to the vehicle's SERIAL1 MAVLink endpoint
|
|
const char *serial1_path = _serial_path[1];
|
|
if (strncmp(serial1_path, "uds:", 4) == 0) {
|
|
gimbal = NEW_NOTHROW SITL::SoloGimbal(serial1_path + 4);
|
|
} else {
|
|
gimbal = NEW_NOTHROW SITL::SoloGimbal(base_port() + 2);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#if AP_SIM_PRECLAND_ENABLED
|
|
// seed the precland simulator's beacon location from home. This
|
|
// is done before parameters are loaded from storage so that the
|
|
// location-is-zero check inside set_default_location sees the
|
|
// unloaded (zero) values; parameters present in storage then
|
|
// overwrite the seed, while any absent ones keep it
|
|
const Location &home = sitl_model->get_home();
|
|
_sitl->precland_sim.set_default_location(home.lat * 1.0e-7f, home.lng * 1.0e-7f, static_cast<int16_t>(sitl_model->get_home_yaw()));
|
|
#endif
|
|
|
|
if (_use_fg_view) {
|
|
fprintf(stdout, "FGView: %s:%u\n", _fg_address, _fg_view_port);
|
|
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;
|
|
_sitl->rcin_path = _rcin_path;
|
|
|
|
fprintf(stdout, "Using \\clock topic for DDS timing: %s\n", _use_dds_sim_time ? "enabled" : "disabled");
|
|
_sitl->use_dds_sim_time = _use_dds_sim_time;
|
|
}
|
|
|
|
// 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
|
|
// most devices can't sleep for 10us - so this is also
|
|
// essentially a yield. At 30x speedup a 10us wall-clock
|
|
// sleep here can equate to your thread sleeping for 300us
|
|
// of simulated time
|
|
usleep(10);
|
|
}
|
|
}
|
|
// 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 < uart->get_system_outqueue_limit()) {
|
|
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;
|
|
if (_model_str != nullptr && strstr(_model_str, "heliquad") != nullptr) {
|
|
// copter variable-pitch quad (heli-quad). The packet has no
|
|
// field for blade collective, so it rides in an unused
|
|
// per-engine field which only the heliquad aircraft model XML
|
|
// reads:
|
|
// rpm[i] - rotor speed, from the shared RSC output
|
|
// fuel_flow[i] - blade collective, -1..1 about trim
|
|
// collective servos are SERVO1-4, RSC is SERVO8 (copter-heli convention)
|
|
const float rsc = constrain_float((pwm_output[7]-1000)*0.001f, 0, 1);
|
|
for (uint8_t i=0; i<4; i++) {
|
|
fdm.rpm[i] = rsc * 1500; // nominal head speed, rev/min
|
|
fdm.fuel_flow[i] = constrain_float((pwm_output[i]-1500)*0.002f, -1, 1);
|
|
}
|
|
} else {
|
|
// normal direct-drive fixed-pitch quadcopter
|
|
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 AP_SIM_JSON_MASTER_ENABLED
|
|
// read servo inputs from ride along flight controllers
|
|
ride_along.receive(input);
|
|
#endif // AP_SIM_JSON_MASTER_ENABLED
|
|
|
|
// 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 AP_SIM_JSON_MASTER_ENABLED
|
|
// output JSON state to ride along flight controllers
|
|
ride_along.send(_sitl->state,sitl_model->get_position_relhome());
|
|
#endif // AP_SIM_JSON_MASTER_ENABLED
|
|
|
|
sim_update();
|
|
|
|
if (_use_fg_view) {
|
|
_output_to_flightgear();
|
|
}
|
|
|
|
// update simulation time
|
|
hal.scheduler->stop_clock(_sitl->state.timestamp_us);
|
|
|
|
set_height_agl();
|
|
|
|
_update_count++;
|
|
}
|
|
|
|
/*
|
|
create sitl_input structure for sending to FDM
|
|
*/
|
|
void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
{
|
|
if (_sitl == nullptr) {
|
|
return;
|
|
}
|
|
|
|
#if AP_SIM_WIND_SIMULATION_ENABLED
|
|
hal.simstate->update_simulated_wind(input);
|
|
#endif // AP_SIM_WIND_SIMULATION_ENABLED
|
|
|
|
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];
|
|
}
|
|
}
|
|
|
|
// 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->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]);
|
|
}
|
|
}
|
|
}
|
|
|
|
#if AP_SIM_VOLZ_ENABLED
|
|
// update simulation input based on data received via "serial" to
|
|
// Volz servos:
|
|
if (_sitl->volz_sim.enabled()) {
|
|
_sitl->volz_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]);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
const float engine_mul = _sitl->engine_mul.get();
|
|
const uint32_t engine_fail = _sitl->engine_fail.get();
|
|
|
|
// apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
|
|
for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
|
|
if (engine_fail & (1<<i)) {
|
|
if (_vehicle != Rover) {
|
|
input.servos[i] = ((input.servos[i]-1000) * engine_mul) + 1000;
|
|
} else {
|
|
input.servos[i] = static_cast<uint16_t>(((input.servos[i] - 1500) * engine_mul) + 1500);
|
|
}
|
|
}
|
|
}
|
|
|
|
float throttle = 0.0f; // 0 is 'no throttle', 1.0 is 'full' throttle
|
|
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) {
|
|
if (input.servos[2] != 0) {
|
|
const uint16_t servo2 = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000));
|
|
throttle = fabsf((servo2 - 1500) / 500.0f);
|
|
} else {
|
|
throttle = 0;
|
|
}
|
|
} 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;
|
|
}
|
|
}
|
|
_sitl->throttle = throttle;
|
|
|
|
set_voltage_current_pins(sitl_model->get_battery_voltage(), sitl_model->get_battery_current());
|
|
}
|
|
|
|
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->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
|
|
|
|
// 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_family = AF_INET;
|
|
|
|
/*
|
|
open the servo input socket; state is also sent from this socket
|
|
so that peripherals can reply to the source address and port they
|
|
observe, whatever this instance's servo port is
|
|
*/
|
|
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);
|
|
}
|
|
|
|
// try to setup for broadcast, this may fail if insufficient privileges
|
|
int one = 1;
|
|
setsockopt(servo_in_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
|
|
|
|
const uint32_t mc_if_addr = sitl_multicast_interface_address();
|
|
if (mc_if_addr != 0) {
|
|
// the state is multicast from this socket, so it needs the same
|
|
// interface pinning the other multicast paths have: without it
|
|
// the state follows the routing table while the peripheral is
|
|
// listening on the interface it was told to use, and never sees
|
|
// the vehicle. See sitl_multicast_interface_address()
|
|
struct in_addr ifaddr {};
|
|
ifaddr.s_addr = mc_if_addr;
|
|
if (setsockopt(servo_in_fd, IPPROTO_IP, IP_MULTICAST_IF, &ifaddr, sizeof(ifaddr)) == -1) {
|
|
fprintf(stderr, "failed to set multicast interface - %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 bind failed\n");
|
|
exit(1);
|
|
}
|
|
|
|
// destination address for the multicast state
|
|
mc_dest = {};
|
|
#ifdef HAVE_SOCK_SIN_LEN
|
|
mc_dest.sin_len = sizeof(mc_dest);
|
|
#endif
|
|
mc_dest.sin_family = AF_INET;
|
|
mc_dest.sin_port = htons(sitl_multicast_state_port(SITL_MCAST_PORT));
|
|
mc_dest.sin_addr.s_addr = inet_addr(SITL_MCAST_IP);
|
|
|
|
::printf("multicast initialised\n");
|
|
}
|
|
|
|
/*
|
|
send out SITL state as multicast UDP
|
|
*/
|
|
void SITL_State::multicast_state_send(void)
|
|
{
|
|
if (_sitl == nullptr) {
|
|
return;
|
|
}
|
|
if (servo_in_fd == -1) {
|
|
multicast_state_open();
|
|
}
|
|
const auto &sfdm = _sitl->state;
|
|
sendto(servo_in_fd, (void*)&sfdm, sizeof(sfdm), 0, (struct sockaddr *)&mc_dest, sizeof(mc_dest));
|
|
|
|
check_servo_input();
|
|
|
|
if (_periph_lockstep) {
|
|
wait_periph_acks(sfdm.timestamp_us);
|
|
}
|
|
}
|
|
|
|
/*
|
|
check for ack/servo data from peripherals
|
|
*/
|
|
void SITL_State::check_servo_input(void)
|
|
{
|
|
// drain any pending packets; we loop to ensure we drain all
|
|
// packets from all nodes
|
|
struct sitl_mcast_ack ack;
|
|
struct sockaddr_in src;
|
|
socklen_t src_len = sizeof(src);
|
|
ssize_t ret;
|
|
while ((ret = recvfrom(servo_in_fd, (void*)&ack, sizeof(ack), MSG_DONTWAIT,
|
|
(struct sockaddr *)&src, &src_len)) > 0) {
|
|
handle_periph_ack(ack, ret, src);
|
|
src_len = sizeof(src);
|
|
}
|
|
}
|
|
|
|
/*
|
|
handle one ack/servo packet from a peripheral
|
|
*/
|
|
void SITL_State::handle_periph_ack(const struct sitl_mcast_ack &ack, ssize_t len, const struct sockaddr_in &src)
|
|
{
|
|
if (len != sizeof(ack)) {
|
|
// unknown packet format. The most likely cause is a
|
|
// peripheral built from a different source tree, sending the
|
|
// old servo-only reply; its servo output will be ignored and
|
|
// it can take no part in lockstep, so say so rather than
|
|
// discarding its packets in silence
|
|
if (!_warned_ack_size) {
|
|
_warned_ack_size = true;
|
|
::fprintf(stderr, "SITL: ignoring %d-byte peripheral reply from %s:%u, expected %u bytes; build the peripheral from this source tree\n",
|
|
int(len), inet_ntoa(src.sin_addr), (unsigned)ntohs(src.sin_port),
|
|
(unsigned)sizeof(ack));
|
|
}
|
|
return;
|
|
}
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
|
|
// nan means that node is not outputting this channel
|
|
if (!isnan(ack.servos[i])) {
|
|
mc_servo[i] = uint16_t(ack.servos[i]);
|
|
}
|
|
}
|
|
if (!_periph_lockstep) {
|
|
return;
|
|
}
|
|
// register the peripheral, or update its ack state
|
|
struct mcast_periph *periph = nullptr;
|
|
for (uint8_t i=0; i<num_mcast_periphs; i++) {
|
|
if (mcast_periphs[i].addr.sin_addr.s_addr == src.sin_addr.s_addr &&
|
|
mcast_periphs[i].addr.sin_port == src.sin_port) {
|
|
periph = &mcast_periphs[i];
|
|
break;
|
|
}
|
|
}
|
|
if (periph == nullptr) {
|
|
// do not let a recently-evicted peer's stale queued acks
|
|
// re-register it; a live peer re-registers with fresh acks
|
|
// once the cooldown expires
|
|
const uint64_t now_ms = wall_millis();
|
|
for (uint8_t i=0; i<num_evicted_periphs; ) {
|
|
if (now_ms - evicted_periphs[i].evicted_ms > PERIPH_REJOIN_COOLDOWN_MS) {
|
|
evicted_periphs[i] = evicted_periphs[--num_evicted_periphs];
|
|
continue;
|
|
}
|
|
if (evicted_periphs[i].addr.sin_addr.s_addr == src.sin_addr.s_addr &&
|
|
evicted_periphs[i].addr.sin_port == src.sin_port) {
|
|
return;
|
|
}
|
|
i++;
|
|
}
|
|
if (num_mcast_periphs >= MAX_MCAST_PERIPHS) {
|
|
return;
|
|
}
|
|
periph = &mcast_periphs[num_mcast_periphs++];
|
|
periph->addr = src;
|
|
::printf("SITL: peripheral %s:%u joined lockstep\n",
|
|
inet_ntoa(src.sin_addr), (unsigned)ntohs(src.sin_port));
|
|
}
|
|
periph->last_ack_us = ack.timestamp_us;
|
|
periph->last_heard_ms = wall_millis();
|
|
}
|
|
|
|
// wall-clock milliseconds; the simulation clock must not be used for
|
|
// the lockstep eviction timeout as it is frozen while we wait
|
|
uint64_t SITL_State::wall_millis(void)
|
|
{
|
|
struct timespec ts;
|
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
|
return uint64_t(ts.tv_sec) * 1000ULL + ts.tv_nsec / 1000000ULL;
|
|
}
|
|
|
|
/*
|
|
strict simulated-peripheral lockstep: do not return (and so do not
|
|
advance the simulation) until every registered peripheral has
|
|
acknowledged consuming the state packet with this timestamp. A
|
|
peripheral which stops responding for a wall-clock second is
|
|
presumed dead (the harness SIGTERMs them with no notice) and is
|
|
evicted; it re-registers on its next ack
|
|
*/
|
|
void SITL_State::wait_periph_acks(const uint64_t timestamp_us)
|
|
{
|
|
while (true) {
|
|
bool all_acked = true;
|
|
const uint64_t now_ms = wall_millis();
|
|
for (uint8_t i=0; i<num_mcast_periphs; ) {
|
|
if (mcast_periphs[i].last_ack_us >= timestamp_us) {
|
|
i++;
|
|
continue;
|
|
}
|
|
if (now_ms - mcast_periphs[i].last_heard_ms > PERIPH_EVICT_TIMEOUT_MS) {
|
|
::fprintf(stderr, "SITL: evicting unresponsive peripheral %s:%u from lockstep\n",
|
|
inet_ntoa(mcast_periphs[i].addr.sin_addr),
|
|
(unsigned)ntohs(mcast_periphs[i].addr.sin_port));
|
|
if (num_evicted_periphs < MAX_MCAST_PERIPHS) {
|
|
evicted_periphs[num_evicted_periphs].addr = mcast_periphs[i].addr;
|
|
evicted_periphs[num_evicted_periphs].evicted_ms = now_ms;
|
|
num_evicted_periphs++;
|
|
}
|
|
mcast_periphs[i] = mcast_periphs[--num_mcast_periphs];
|
|
continue;
|
|
}
|
|
all_acked = false;
|
|
i++;
|
|
}
|
|
if (all_acked) {
|
|
return;
|
|
}
|
|
struct pollfd pfd { servo_in_fd, POLLIN, 0 };
|
|
poll(&pfd, 1, PERIPH_ACK_POLL_MS);
|
|
check_servo_input();
|
|
}
|
|
}
|
|
|
|
/*
|
|
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
|