mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-27 02:04:00 -04:00
HAL_SITL: use a synthetic clock when possible
this decouples wall clock time from simulation time if the FDM supports it
This commit is contained in:
Andrew Tridgell
parent
b4df5b35f0
commit
9748cb1e3e
@ -10,7 +10,6 @@
|
||||
hal.scheduler->system_initialized(); \
|
||||
for(;;) { \
|
||||
loop(); \
|
||||
AVR_SITL::SITL_State::loop_hook(); \
|
||||
} \
|
||||
return 0;\
|
||||
}\
|
||||
|
||||
@ -20,6 +20,9 @@
|
||||
#include <sys/select.h>
|
||||
|
||||
#include <AP_Param.h>
|
||||
#include <pthread.h>
|
||||
|
||||
typedef void *(*pthread_startroutine_t)(void *);
|
||||
|
||||
#ifdef __CYGWIN__
|
||||
#include <stdio.h>
|
||||
@ -49,37 +52,11 @@ extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace AVR_SITL;
|
||||
|
||||
enum SITL_State::vehicle_type SITL_State::_vehicle;
|
||||
uint16_t SITL_State::_framerate;
|
||||
uint16_t SITL_State::_base_port = 5760;
|
||||
uint16_t SITL_State::_rcout_port = 5502;
|
||||
uint16_t SITL_State::_simin_port = 5501;
|
||||
struct sockaddr_in SITL_State::_rcout_addr;
|
||||
pid_t SITL_State::_parent_pid;
|
||||
uint32_t SITL_State::_update_count;
|
||||
bool SITL_State::_motors_on;
|
||||
uint16_t SITL_State::sonar_pin_value;
|
||||
uint16_t SITL_State::airspeed_pin_value;
|
||||
uint16_t SITL_State::voltage_pin_value;
|
||||
uint16_t SITL_State::current_pin_value;
|
||||
float SITL_State::_current;
|
||||
|
||||
AP_Baro *SITL_State::_barometer;
|
||||
AP_InertialSensor *SITL_State::_ins;
|
||||
SITLScheduler *SITL_State::_scheduler;
|
||||
Compass *SITL_State::_compass;
|
||||
OpticalFlow *SITL_State::_optical_flow;
|
||||
AP_Terrain *SITL_State::_terrain;
|
||||
|
||||
int SITL_State::_sitl_fd;
|
||||
SITL *SITL_State::_sitl;
|
||||
uint16_t SITL_State::pwm_output[11];
|
||||
uint16_t SITL_State::last_pwm_output[11];
|
||||
uint16_t SITL_State::pwm_input[8];
|
||||
bool SITL_State::new_rc_input;
|
||||
// this allows loop_hook to be called
|
||||
SITL_State *g_state;
|
||||
|
||||
// catch floating point exceptions
|
||||
void SITL_State::_sig_fpe(int signum)
|
||||
static void _sig_fpe(int signum)
|
||||
{
|
||||
fprintf(stderr, "ERROR: Floating point exception - aborting\n");
|
||||
abort();
|
||||
@ -106,7 +83,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
|
||||
setvbuf(stdout, (char *)0, _IONBF, 0);
|
||||
setvbuf(stderr, (char *)0, _IONBF, 0);
|
||||
|
||||
while ((opt = getopt(argc, argv, "swhr:H:CI:P:")) != -1) {
|
||||
_synthetic_clock_mode = false;
|
||||
_base_port = 5760;
|
||||
_rcout_port = 5502;
|
||||
_simin_port = 5501;
|
||||
|
||||
while ((opt = getopt(argc, argv, "swhr:H:CI:P:S")) != -1) {
|
||||
switch (opt) {
|
||||
case 'w':
|
||||
AP_Param::erase_all();
|
||||
@ -131,6 +113,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
|
||||
case 'P':
|
||||
_set_param_default(optarg);
|
||||
break;
|
||||
case 'S':
|
||||
_synthetic_clock_mode = true;
|
||||
break;
|
||||
default:
|
||||
_usage();
|
||||
exit(1);
|
||||
@ -205,7 +190,6 @@ void SITL_State::_sitl_setup(void)
|
||||
_rcout_addr.sin_port = htons(_rcout_port);
|
||||
inet_pton(AF_INET, "127.0.0.1", &_rcout_addr.sin_addr);
|
||||
|
||||
_setup_timer();
|
||||
#ifndef HIL_MODE
|
||||
_setup_fdm();
|
||||
#endif
|
||||
@ -228,6 +212,28 @@ void SITL_State::_sitl_setup(void)
|
||||
_update_gps(0, 0, 0, 0, 0, 0, false);
|
||||
#endif
|
||||
}
|
||||
|
||||
if (_synthetic_clock_mode) {
|
||||
// start with non-zero clock
|
||||
hal.scheduler->stop_clock(100);
|
||||
}
|
||||
|
||||
// setup a pipe used to trigger loop to stop sleeping
|
||||
pipe(_fdm_pipe);
|
||||
AVR_SITL::SITLUARTDriver::_set_nonblocking(_fdm_pipe[0]);
|
||||
AVR_SITL::SITLUARTDriver::_set_nonblocking(_fdm_pipe[1]);
|
||||
|
||||
g_state = this;
|
||||
|
||||
/*
|
||||
setup thread that receives input from the FDM
|
||||
*/
|
||||
pthread_attr_t thread_attr;
|
||||
pthread_attr_init(&thread_attr);
|
||||
|
||||
pthread_create(&_fdm_thread_ctx, &thread_attr,
|
||||
(pthread_startroutine_t)&AVR_SITL::SITL_State::_fdm_thread, this);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -270,96 +276,89 @@ void SITL_State::_setup_fdm(void)
|
||||
|
||||
|
||||
/*
|
||||
timer called at 1kHz
|
||||
thread for FDM input
|
||||
*/
|
||||
void SITL_State::_timer_handler(int signum)
|
||||
void SITL_State::_fdm_thread(void)
|
||||
{
|
||||
static uint32_t last_update_count;
|
||||
static uint32_t last_pwm_input;
|
||||
uint32_t last_update_count = 0;
|
||||
uint32_t last_pwm_input = 0;
|
||||
|
||||
static bool in_timer;
|
||||
while (true) {
|
||||
fd_set fds;
|
||||
struct timeval tv;
|
||||
|
||||
if (in_timer || _scheduler->interrupts_are_blocked() || _sitl == NULL){
|
||||
return;
|
||||
}
|
||||
if (next_stop_clock != 0) {
|
||||
hal.scheduler->stop_clock(next_stop_clock);
|
||||
next_stop_clock = 0;
|
||||
}
|
||||
|
||||
_scheduler->sitl_begin_atomic();
|
||||
in_timer = true;
|
||||
tv.tv_sec = 1;
|
||||
tv.tv_usec = 0;
|
||||
|
||||
#ifndef __CYGWIN__
|
||||
/* make sure we die if our parent dies */
|
||||
if (kill(_parent_pid, 0) != 0) {
|
||||
exit(1);
|
||||
}
|
||||
#else
|
||||
|
||||
static uint16_t count = 0;
|
||||
static uint32_t last_report;
|
||||
|
||||
count++;
|
||||
if (hal.scheduler->millis() - last_report > 1000) {
|
||||
fprintf(stdout, "TH %u cps\n", count);
|
||||
// print_trace();
|
||||
count = 0;
|
||||
last_report = hal.scheduler->millis();
|
||||
}
|
||||
#endif
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(_sitl_fd, &fds);
|
||||
if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
|
||||
// internal error
|
||||
_simulator_output(true);
|
||||
continue;
|
||||
}
|
||||
|
||||
// simulate RC input at 50Hz
|
||||
if (hal.scheduler->millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) {
|
||||
last_pwm_input = hal.scheduler->millis();
|
||||
new_rc_input = true;
|
||||
}
|
||||
/* check for packet from flight sim */
|
||||
_fdm_input();
|
||||
|
||||
#ifndef HIL_MODE
|
||||
/* check for packet from flight sim */
|
||||
_fdm_input();
|
||||
/* make sure we die if our parent dies */
|
||||
if (kill(_parent_pid, 0) != 0) {
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// send RC output to flight sim
|
||||
_simulator_output();
|
||||
#endif
|
||||
if (_scheduler->interrupts_are_blocked() || _sitl == NULL) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (_update_count == 0 && _sitl != NULL) {
|
||||
#ifndef HIL_MODE
|
||||
_update_gps(0, 0, 0, 0, 0, 0, false);
|
||||
_update_barometer(0);
|
||||
#endif
|
||||
_scheduler->timer_event();
|
||||
// simulate RC input at 50Hz
|
||||
if (hal.scheduler->millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) {
|
||||
last_pwm_input = hal.scheduler->millis();
|
||||
new_rc_input = true;
|
||||
}
|
||||
|
||||
_scheduler->sitl_begin_atomic();
|
||||
|
||||
if (_update_count == 0 && _sitl != NULL) {
|
||||
_update_gps(0, 0, 0, 0, 0, 0, false);
|
||||
_update_barometer(0);
|
||||
_scheduler->timer_event();
|
||||
_scheduler->sitl_end_atomic();
|
||||
continue;
|
||||
}
|
||||
|
||||
if (_update_count == last_update_count) {
|
||||
_scheduler->timer_event();
|
||||
_scheduler->sitl_end_atomic();
|
||||
continue;
|
||||
}
|
||||
last_update_count = _update_count;
|
||||
|
||||
if (_sitl != NULL) {
|
||||
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
|
||||
_sitl->state.altitude,
|
||||
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
|
||||
!_sitl->gps_disable);
|
||||
_update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
|
||||
_sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
|
||||
_sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
|
||||
_sitl->state.airspeed, _sitl->state.altitude);
|
||||
_update_barometer(_sitl->state.altitude);
|
||||
_update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
|
||||
_update_flow();
|
||||
}
|
||||
|
||||
// trigger all APM timers.
|
||||
_scheduler->timer_event();
|
||||
_scheduler->sitl_end_atomic();
|
||||
in_timer = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (_update_count == last_update_count) {
|
||||
_scheduler->timer_event();
|
||||
_scheduler->sitl_end_atomic();
|
||||
in_timer = false;
|
||||
return;
|
||||
}
|
||||
last_update_count = _update_count;
|
||||
|
||||
if (_sitl != NULL) {
|
||||
#ifndef HIL_MODE
|
||||
_update_gps(_sitl->state.latitude, _sitl->state.longitude,
|
||||
_sitl->state.altitude,
|
||||
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
|
||||
!_sitl->gps_disable);
|
||||
_update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
|
||||
_sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
|
||||
_sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
|
||||
_sitl->state.airspeed, _sitl->state.altitude);
|
||||
_update_barometer(_sitl->state.altitude);
|
||||
_update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
|
||||
_update_flow();
|
||||
#endif
|
||||
char b = 0;
|
||||
write(_fdm_pipe[1], &b, 1);
|
||||
}
|
||||
|
||||
// trigger all APM timers. We do this last as it can re-enable
|
||||
// interrupts, which can lead to recursion
|
||||
_scheduler->timer_event();
|
||||
|
||||
_scheduler->sitl_end_atomic();
|
||||
in_timer = false;
|
||||
}
|
||||
|
||||
#ifndef HIL_MODE
|
||||
@ -373,12 +372,25 @@ void SITL_State::_fdm_input(void)
|
||||
uint16_t pwm[8];
|
||||
};
|
||||
union {
|
||||
struct {
|
||||
uint64_t timestamp;
|
||||
struct sitl_fdm fg_pkt;
|
||||
} fg_pkt_timestamped;
|
||||
struct sitl_fdm fg_pkt;
|
||||
struct pwm_packet pwm_pkt;
|
||||
} d;
|
||||
bool got_fg_input = false;
|
||||
next_stop_clock = 0;
|
||||
|
||||
size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
|
||||
switch (size) {
|
||||
case 148:
|
||||
/* a fg packate with a timestamp */
|
||||
next_stop_clock = d.fg_pkt_timestamped.timestamp;
|
||||
memmove(&d.fg_pkt, &d.fg_pkt_timestamped.fg_pkt, sizeof(d.fg_pkt));
|
||||
_synthetic_clock_mode = true;
|
||||
// fall through
|
||||
|
||||
case 140:
|
||||
static uint32_t last_report;
|
||||
static uint32_t count;
|
||||
@ -388,6 +400,8 @@ void SITL_State::_fdm_input(void)
|
||||
return;
|
||||
}
|
||||
|
||||
got_fg_input = true;
|
||||
|
||||
if (d.fg_pkt.latitude == 0 ||
|
||||
d.fg_pkt.longitude == 0 ||
|
||||
d.fg_pkt.altitude <= 0) {
|
||||
@ -428,6 +442,10 @@ void SITL_State::_fdm_input(void)
|
||||
}
|
||||
}
|
||||
|
||||
if (got_fg_input) {
|
||||
// send RC output to flight sim
|
||||
_simulator_output(_synthetic_clock_mode);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -460,7 +478,7 @@ void SITL_State::_apply_servo_filter(float deltat)
|
||||
/*
|
||||
send RC outputs to simulator
|
||||
*/
|
||||
void SITL_State::_simulator_output(void)
|
||||
void SITL_State::_simulator_output(bool synthetic_clock_mode)
|
||||
{
|
||||
static uint32_t last_update_usec;
|
||||
struct {
|
||||
@ -495,7 +513,7 @@ void SITL_State::_simulator_output(void)
|
||||
|
||||
// output at chosen framerate
|
||||
uint32_t now = hal.scheduler->micros();
|
||||
if (last_update_usec != 0 && now - last_update_usec < 1000000/_framerate) {
|
||||
if (!synthetic_clock_mode && last_update_usec != 0 && now - last_update_usec < 1000000/_framerate) {
|
||||
return;
|
||||
}
|
||||
float deltat = (now - last_update_usec) * 1.0e-6f;
|
||||
@ -577,28 +595,6 @@ void SITL_State::_simulator_output(void)
|
||||
sendto(_sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&_rcout_addr, sizeof(_rcout_addr));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
setup a timer used to prod the ISRs
|
||||
*/
|
||||
void SITL_State::_setup_timer(void)
|
||||
{
|
||||
struct itimerval it;
|
||||
struct sigaction act;
|
||||
|
||||
act.sa_handler = _timer_handler;
|
||||
act.sa_flags = SA_RESTART|SA_NODEFER;
|
||||
sigemptyset(&act.sa_mask);
|
||||
sigaddset(&act.sa_mask, SIGALRM);
|
||||
sigaction(SIGALRM, &act, NULL);
|
||||
|
||||
it.it_interval.tv_sec = 0;
|
||||
it.it_interval.tv_usec = 1000; // 1KHz
|
||||
it.it_value = it.it_interval;
|
||||
|
||||
setitimer(ITIMER_REAL, &it, NULL);
|
||||
}
|
||||
|
||||
// generate a random float between -1 and 1
|
||||
float SITL_State::_rand_float(void)
|
||||
{
|
||||
@ -661,11 +657,20 @@ void SITL_State::loop_hook(void)
|
||||
FD_SET(fd, &fds);
|
||||
max_fd = max(fd, max_fd);
|
||||
}
|
||||
|
||||
FD_SET(_fdm_pipe[0], &fds);
|
||||
max_fd = max(_fdm_pipe[0], max_fd);
|
||||
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 100;
|
||||
fflush(stdout);
|
||||
fflush(stderr);
|
||||
select(max_fd+1, &fds, NULL, NULL, &tv);
|
||||
|
||||
if (FD_ISSET(_fdm_pipe[0], &fds)) {
|
||||
char b;
|
||||
read(_fdm_pipe[0], &b, 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -39,18 +39,18 @@ public:
|
||||
int gps_pipe(void);
|
||||
int gps2_pipe(void);
|
||||
ssize_t gps_read(int fd, void *buf, size_t count);
|
||||
static uint16_t pwm_output[11];
|
||||
static uint16_t last_pwm_output[11];
|
||||
static uint16_t pwm_input[8];
|
||||
static bool new_rc_input;
|
||||
static void loop_hook(void);
|
||||
uint16_t pwm_output[11];
|
||||
uint16_t last_pwm_output[11];
|
||||
uint16_t pwm_input[8];
|
||||
bool new_rc_input;
|
||||
void loop_hook(void);
|
||||
uint16_t base_port(void) const { return _base_port; }
|
||||
|
||||
// simulated airspeed, sonar and battery monitor
|
||||
static uint16_t sonar_pin_value; // pin 0
|
||||
static uint16_t airspeed_pin_value; // pin 1
|
||||
static uint16_t voltage_pin_value; // pin 13
|
||||
static uint16_t current_pin_value; // pin 12
|
||||
uint16_t sonar_pin_value; // pin 0
|
||||
uint16_t airspeed_pin_value; // pin 1
|
||||
uint16_t voltage_pin_value; // pin 13
|
||||
uint16_t current_pin_value; // pin 12
|
||||
|
||||
private:
|
||||
void _parse_command_line(int argc, char * const argv[]);
|
||||
@ -61,12 +61,10 @@ private:
|
||||
void _setup_timer(void);
|
||||
void _setup_adc(void);
|
||||
|
||||
// these methods are static as they are called
|
||||
// from the timer
|
||||
static float height_agl(void);
|
||||
static void _update_barometer(float height);
|
||||
static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
|
||||
static void _update_flow(void);
|
||||
float height_agl(void);
|
||||
void _update_barometer(float height);
|
||||
void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
|
||||
void _update_flow(void);
|
||||
|
||||
struct gps_data {
|
||||
double latitude;
|
||||
@ -79,64 +77,67 @@ private:
|
||||
};
|
||||
|
||||
#define MAX_GPS_DELAY 100
|
||||
static gps_data _gps_data[MAX_GPS_DELAY];
|
||||
gps_data _gps_data[MAX_GPS_DELAY];
|
||||
|
||||
static bool _gps_has_basestation_position;
|
||||
static gps_data _gps_basestation_data;
|
||||
static void _gps_write(const uint8_t *p, uint16_t size);
|
||||
static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
|
||||
static void _update_gps_ubx(const struct gps_data *d);
|
||||
static void _update_gps_mtk(const struct gps_data *d);
|
||||
static void _update_gps_mtk16(const struct gps_data *d);
|
||||
static void _update_gps_mtk19(const struct gps_data *d);
|
||||
static uint16_t _gps_nmea_checksum(const char *s);
|
||||
static void _gps_nmea_printf(const char *fmt, ...);
|
||||
static void _update_gps_nmea(const struct gps_data *d);
|
||||
static void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
|
||||
static void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
|
||||
bool _gps_has_basestation_position;
|
||||
gps_data _gps_basestation_data;
|
||||
void _gps_write(const uint8_t *p, uint16_t size);
|
||||
void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
|
||||
void _update_gps_ubx(const struct gps_data *d);
|
||||
void _update_gps_mtk(const struct gps_data *d);
|
||||
void _update_gps_mtk16(const struct gps_data *d);
|
||||
void _update_gps_mtk19(const struct gps_data *d);
|
||||
uint16_t _gps_nmea_checksum(const char *s);
|
||||
void _gps_nmea_printf(const char *fmt, ...);
|
||||
void _update_gps_nmea(const struct gps_data *d);
|
||||
void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
|
||||
void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
|
||||
|
||||
static void _update_gps(double latitude, double longitude, float altitude,
|
||||
void _update_gps(double latitude, double longitude, float altitude,
|
||||
double speedN, double speedE, double speedD, bool have_lock);
|
||||
|
||||
static void _update_ins(float roll, float pitch, float yaw, // Relative to earth
|
||||
void _update_ins(float roll, float pitch, float yaw, // Relative to earth
|
||||
double rollRate, double pitchRate,double yawRate, // Local to plane
|
||||
double xAccel, double yAccel, double zAccel, // Local to plane
|
||||
float airspeed, float altitude);
|
||||
static void _fdm_input(void);
|
||||
static void _simulator_output(void);
|
||||
static void _apply_servo_filter(float deltat);
|
||||
static uint16_t _airspeed_sensor(float airspeed);
|
||||
static uint16_t _ground_sonar();
|
||||
static float _gyro_drift(void);
|
||||
static float _rand_float(void);
|
||||
static Vector3f _rand_vec3f(void);
|
||||
void _fdm_input(void);
|
||||
void _simulator_output(bool synthetic_clock_mode);
|
||||
void _apply_servo_filter(float deltat);
|
||||
uint16_t _airspeed_sensor(float airspeed);
|
||||
uint16_t _ground_sonar();
|
||||
float _gyro_drift(void);
|
||||
float _rand_float(void);
|
||||
Vector3f _rand_vec3f(void);
|
||||
|
||||
// signal handlers
|
||||
static void _sig_fpe(int signum);
|
||||
static void _timer_handler(int signum);
|
||||
pthread_t _fdm_thread_ctx;
|
||||
void _fdm_thread(void);
|
||||
int _fdm_pipe[2];
|
||||
uint64_t next_stop_clock;
|
||||
|
||||
// internal state
|
||||
static enum vehicle_type _vehicle;
|
||||
static uint16_t _framerate;
|
||||
static uint16_t _base_port;
|
||||
enum vehicle_type _vehicle;
|
||||
uint16_t _framerate;
|
||||
uint16_t _base_port;
|
||||
float _initial_height;
|
||||
static struct sockaddr_in _rcout_addr;
|
||||
static pid_t _parent_pid;
|
||||
static uint32_t _update_count;
|
||||
static bool _motors_on;
|
||||
struct sockaddr_in _rcout_addr;
|
||||
pid_t _parent_pid;
|
||||
uint32_t _update_count;
|
||||
bool _motors_on;
|
||||
|
||||
static AP_Baro *_barometer;
|
||||
static AP_InertialSensor *_ins;
|
||||
static SITLScheduler *_scheduler;
|
||||
static Compass *_compass;
|
||||
static OpticalFlow *_optical_flow;
|
||||
static AP_Terrain *_terrain;
|
||||
AP_Baro *_barometer;
|
||||
AP_InertialSensor *_ins;
|
||||
SITLScheduler *_scheduler;
|
||||
Compass *_compass;
|
||||
OpticalFlow *_optical_flow;
|
||||
AP_Terrain *_terrain;
|
||||
|
||||
static int _sitl_fd;
|
||||
static SITL *_sitl;
|
||||
static uint16_t _rcout_port;
|
||||
static uint16_t _simin_port;
|
||||
static float _current;
|
||||
int _sitl_fd;
|
||||
SITL *_sitl;
|
||||
uint16_t _rcout_port;
|
||||
uint16_t _simin_port;
|
||||
float _current;
|
||||
|
||||
bool _synthetic_clock_mode;
|
||||
};
|
||||
|
||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
|
||||
@ -8,6 +8,8 @@
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
#include <fenv.h>
|
||||
#include <signal.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#ifdef __CYGWIN__
|
||||
#include <stdio.h>
|
||||
@ -44,9 +46,16 @@ double SITLScheduler::_cyg_freq = 0;
|
||||
long SITLScheduler::_cyg_start = 0;
|
||||
#endif
|
||||
|
||||
static void sigcont_handler(int)
|
||||
{
|
||||
}
|
||||
|
||||
SITLScheduler::SITLScheduler(SITL_State *sitlState) :
|
||||
_sitlState(sitlState)
|
||||
{}
|
||||
_sitlState(sitlState),
|
||||
stopped_clock_usec(0)
|
||||
{
|
||||
signal(SIGCONT, sigcont_handler);
|
||||
}
|
||||
|
||||
void SITLScheduler::init(void *unused)
|
||||
{
|
||||
@ -88,6 +97,9 @@ uint64_t SITLScheduler::_micros64()
|
||||
|
||||
uint64_t SITLScheduler::micros64()
|
||||
{
|
||||
if (stopped_clock_usec) {
|
||||
return stopped_clock_usec;
|
||||
}
|
||||
return _micros64();
|
||||
}
|
||||
|
||||
@ -98,6 +110,9 @@ uint32_t SITLScheduler::micros()
|
||||
|
||||
uint64_t SITLScheduler::millis64()
|
||||
{
|
||||
if (stopped_clock_usec) {
|
||||
return stopped_clock_usec/1000;
|
||||
}
|
||||
#ifdef __CYGWIN__
|
||||
// 1000 ms in a second
|
||||
return (uint64_t)(_cyg_sec() * 1000);
|
||||
@ -116,24 +131,29 @@ uint32_t SITLScheduler::millis()
|
||||
return millis64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
extern AVR_SITL::SITL_State *g_state;
|
||||
|
||||
|
||||
void SITLScheduler::delay_microseconds(uint16_t usec)
|
||||
{
|
||||
uint64_t start = micros64();
|
||||
while (micros64() - start < usec) {
|
||||
usleep(usec - (micros64() - start));
|
||||
uint64_t dtime;
|
||||
while ((dtime=(micros64() - start) < usec)) {
|
||||
if (stopped_clock_usec) {
|
||||
wait_time_usec = start + usec;
|
||||
pause();
|
||||
wait_time_usec = 0;
|
||||
} else {
|
||||
usleep(usec - dtime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SITLScheduler::delay(uint16_t ms)
|
||||
{
|
||||
uint64_t start = micros64();
|
||||
|
||||
while (ms > 0) {
|
||||
while ((micros64() - start) >= 1000) {
|
||||
ms--;
|
||||
if (ms == 0) break;
|
||||
start += 1000;
|
||||
}
|
||||
delay_microseconds(1000);
|
||||
ms--;
|
||||
if (_min_delay_cb_ms <= ms) {
|
||||
if (_delay_cb) {
|
||||
_delay_cb();
|
||||
@ -294,4 +314,24 @@ void SITLScheduler::panic(const prog_char_t *errormsg) {
|
||||
for(;;);
|
||||
}
|
||||
|
||||
/*
|
||||
set simulation timestamp
|
||||
*/
|
||||
void SITLScheduler::stop_clock(uint64_t time_usec)
|
||||
{
|
||||
stopped_clock_usec = time_usec;
|
||||
/*
|
||||
we want to ensure the main thread
|
||||
*/
|
||||
while (wait_time_usec == 0) {
|
||||
pthread_yield();
|
||||
}
|
||||
kill(0, SIGCONT);
|
||||
while (wait_time_usec != 0 && wait_time_usec <= time_usec) {
|
||||
kill(0, SIGCONT);
|
||||
pthread_yield();
|
||||
}
|
||||
_run_io_procs(false);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -73,8 +73,11 @@ private:
|
||||
static double _cyg_sec();
|
||||
#endif
|
||||
|
||||
bool _initialized;
|
||||
void stop_clock(uint64_t time_usec);
|
||||
|
||||
bool _initialized;
|
||||
volatile uint64_t stopped_clock_usec;
|
||||
volatile uint64_t wait_time_usec;
|
||||
};
|
||||
#endif
|
||||
#endif // __AP_HAL_SITL_SCHEDULER_H__
|
||||
|
||||
@ -32,9 +32,6 @@ extern const AP_HAL::HAL& hal;
|
||||
|
||||
static uint8_t next_gps_index;
|
||||
static uint8_t gps_delay;
|
||||
SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY];
|
||||
bool SITL_State::_gps_has_basestation_position = false;
|
||||
SITL_State::gps_data SITL_State::_gps_basestation_data;
|
||||
|
||||
// state of GPS emulation
|
||||
static struct gps_state {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user