Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: removed the need for the FDM thread

Browse Source 
this also removes the need for the barriers
This commit is contained in:
Andrew Tridgell 2015-05-02 21:41:13 +10:00
parent 19147a05f6
commit a6f41b3ca6
4 changed files with 82 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -20,12 +20,9 @@
#include <sys/select.h> #include <sys/select.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <pthread.h>
#include <SIM_Multicopter.h> #include <SIM_Multicopter.h>
typedef void *(*pthread_startroutine_t)(void *);
#ifdef __CYGWIN__ #ifdef __CYGWIN__
#include <stdio.h> #include <stdio.h>
#include <signal.h> #include <signal.h>
@ -54,7 +51,6 @@ extern const AP_HAL::HAL& hal;
using namespace AVR_SITL; using namespace AVR_SITL;
// this allows loop_hook to be called
SITL_State *g_state; SITL_State *g_state;
// catch floating point exceptions // catch floating point exceptions
@ -72,6 +68,9 @@ void SITL_State::_usage(void)
fprintf(stdout, "\t-H HEIGHT initial barometric height\n"); fprintf(stdout, "\t-H HEIGHT initial barometric height\n");
fprintf(stdout, "\t-C use console instead of TCP ports\n"); fprintf(stdout, "\t-C use console instead of TCP ports\n");
fprintf(stdout, "\t-I set instance of SITL (adds 10*instance to all port numbers)\n"); fprintf(stdout, "\t-I set instance of SITL (adds 10*instance to all port numbers)\n");
fprintf(stdout, "\t-s SPEEDUP simulation speedup\n");
fprintf(stdout, "\t-O ORIGIN set home location (lat,lng,alt,yaw)\n");
fprintf(stdout, "\t-M MODEL set simulation model\n");
} }
void SITL_State::_parse_command_line(int argc, char * const argv[]) void SITL_State::_parse_command_line(int argc, char * const argv[])
@ -79,6 +78,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
int opt; int opt;
const char *home_str = NULL; const char *home_str = NULL;
const char *model_str = NULL; const char *model_str = NULL;
float speedup = 1.0f;
signal(SIGFPE, _sig_fpe); signal(SIGFPE, _sig_fpe);
// No-op SIGPIPE handler // No-op SIGPIPE handler
@ -92,7 +92,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_rcout_port = 5502; _rcout_port = 5502;
_simin_port = 5501; _simin_port = 5501;
while ((opt = getopt(argc, argv, "swhr:H:CI:P:SO:M:")) != -1) { while ((opt = getopt(argc, argv, "s:whr:H:CI:P:SO:M:S:")) != -1) {
switch (opt) { switch (opt) {
case 'w': case 'w':
AP_Param::erase_all(); AP_Param::erase_all();
@ -125,6 +125,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
break; break;
case 'M': case 'M':
model_str = optarg; model_str = optarg;
break;
case 's':
speedup = atof(optarg);
break; break;
default: default:
_usage(); _usage();
@ -134,7 +137,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
if (model_str && home_str) { if (model_str && home_str) {
sitl_model = new MultiCopter(home_str, model_str); sitl_model = new MultiCopter(home_str, model_str);
printf("Started model %s at %s\n", model_str, home_str); sitl_model->set_speedup(speedup);
_synthetic_clock_mode = true;
printf("Started model %s at %s at speed %.1f\n", model_str, home_str, speedup);
} }
fprintf(stdout, "Starting sketch '%s'\n", SKETCH); fprintf(stdout, "Starting sketch '%s'\n", SKETCH);
@ -233,22 +238,8 @@ void SITL_State::_sitl_setup(void)
hal.scheduler->stop_clock(1); hal.scheduler->stop_clock(1);
} }
// setup a pipe used to trigger loop to stop sleeping // keep a global state pointer for static callbacks
pipe(_fdm_pipe);
AVR_SITL::SITLUARTDriver::_set_nonblocking(_fdm_pipe[0]);
AVR_SITL::SITLUARTDriver::_set_nonblocking(_fdm_pipe[1]);
g_state = this; 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);
} }
@ -291,92 +282,81 @@ void SITL_State::_setup_fdm(void)
/* /*
thread for FDM input step the FDM by one time step
*/ */
void SITL_State::_fdm_thread(void) void SITL_State::_fdm_input_step(void)
{ {
uint32_t last_update_count = 0; static uint32_t last_pwm_input = 0;
uint32_t last_pwm_input = 0; fd_set fds;
struct timeval tv;
while (true) { if (sitl_model != NULL) {
fd_set fds; _fdm_input_local();
struct timeval tv; } else {
tv.tv_sec = 1;
if (next_stop_clock != 0) { tv.tv_usec = 0;
hal.scheduler->stop_clock(next_stop_clock);
next_stop_clock = 0; FD_ZERO(&fds);
} FD_SET(_sitl_fd, &fds);
if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
if (sitl_model != NULL) { // internal error
_fdm_input_local(); _simulator_output(true);
} else { return;
tv.tv_sec = 1;
tv.tv_usec = 0;
FD_ZERO(&fds);
FD_SET(_sitl_fd, &fds);
if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
// internal error
_simulator_output(true);
continue;
}
/* check for packet from flight sim */
_fdm_input();
}
/* make sure we die if our parent dies */
if (kill(_parent_pid, 0) != 0) {
exit(1);
} }
if (_scheduler->interrupts_are_blocked() || _sitl == NULL) { /* check for packet from flight sim */
continue; _fdm_input();
} }
// simulate RC input at 50Hz /* make sure we die if our parent dies */
if (hal.scheduler->millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) { if (kill(_parent_pid, 0) != 0) {
last_pwm_input = hal.scheduler->millis(); exit(1);
new_rc_input = true; }
}
if (_scheduler->interrupts_are_blocked() || _sitl == NULL) {
return;
}
_scheduler->sitl_begin_atomic(); // 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;
}
if (_update_count == 0 && _sitl != NULL) { _scheduler->sitl_begin_atomic();
_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) { if (_update_count == 0 && _sitl != NULL) {
_scheduler->timer_event(); _update_gps(0, 0, 0, 0, 0, 0, false);
_scheduler->sitl_end_atomic(); _update_barometer(0);
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->timer_event();
_scheduler->sitl_end_atomic(); _scheduler->sitl_end_atomic();
return;
}
char b = 0; if (_sitl != NULL) {
write(_fdm_pipe[1], &b, 1); _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();
}
void SITL_State::wait_clock(uint64_t wait_time_usec)
{
while (hal.scheduler->micros64() < wait_time_usec) {
_fdm_input_step();
} }
} }
@ -395,7 +375,6 @@ void SITL_State::_fdm_input(void)
struct pwm_packet pwm_pkt; struct pwm_packet pwm_pkt;
} d; } d;
bool got_fg_input = false; bool got_fg_input = false;
next_stop_clock = 0;
size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT); size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
switch (size) { switch (size) {
@ -408,7 +387,7 @@ void SITL_State::_fdm_input(void)
return; return;
} }
next_stop_clock = d.fg_pkt.timestamp_us; hal.scheduler->stop_clock(d.fg_pkt.timestamp_us);
_synthetic_clock_mode = true; _synthetic_clock_mode = true;
got_fg_input = true; got_fg_input = true;
@ -469,7 +448,7 @@ void SITL_State::_fdm_input_local(void)
_simulator_servos(input); _simulator_servos(input);
sitl_model->update(input); sitl_model->update(input);
sitl_model->fill_fdm(_sitl->state); sitl_model->fill_fdm(_sitl->state);
next_stop_clock = _sitl->state.timestamp_us; hal.scheduler->stop_clock(_sitl->state.timestamp_us);
_synthetic_clock_mode = true; _synthetic_clock_mode = true;
_update_count++; _update_count++;
} }
@ -663,56 +642,6 @@ void SITL_State::init(int argc, char * const argv[])
_parse_command_line(argc, argv); _parse_command_line(argc, argv);
} }
// wait for serial input, or 100usec
void SITL_State::loop_hook(void)
{
struct timeval tv;
fd_set fds;
int fd, max_fd = 0;
FD_ZERO(&fds);
fd = ((AVR_SITL::SITLUARTDriver*)hal.uartA)->_fd;
if (fd != -1) {
FD_SET(fd, &fds);
max_fd = max(fd, max_fd);
}
fd = ((AVR_SITL::SITLUARTDriver*)hal.uartB)->_fd;
if (fd != -1) {
FD_SET(fd, &fds);
max_fd = max(fd, max_fd);
}
fd = ((AVR_SITL::SITLUARTDriver*)hal.uartC)->_fd;
if (fd != -1) {
FD_SET(fd, &fds);
max_fd = max(fd, max_fd);
}
fd = ((AVR_SITL::SITLUARTDriver*)hal.uartD)->_fd;
if (fd != -1) {
FD_SET(fd, &fds);
max_fd = max(fd, max_fd);
}
fd = ((AVR_SITL::SITLUARTDriver*)hal.uartE)->_fd;
if (fd != -1) {
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);
}
}
/* /*
return height above the ground in meters return height above the ground in meters
*/ */

5
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -111,11 +111,12 @@ private:
float _gyro_drift(void); float _gyro_drift(void);
float _rand_float(void); float _rand_float(void);
Vector3f _rand_vec3f(void); Vector3f _rand_vec3f(void);
void _fdm_input_step(void);
void wait_clock(uint64_t wait_time_usec);
pthread_t _fdm_thread_ctx; pthread_t _fdm_thread_ctx;
void _fdm_thread(void); void _fdm_thread(void);
int _fdm_pipe[2];
uint64_t next_stop_clock;
// internal state // internal state
enum vehicle_type _vehicle; enum vehicle_type _vehicle;

64
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -30,18 +30,10 @@ bool SITLScheduler::_in_io_proc = false;
struct timeval SITLScheduler::_sketch_start_time; struct timeval SITLScheduler::_sketch_start_time;
static void sigcont_handler(int)
{
}
SITLScheduler::SITLScheduler(SITL_State *sitlState) : SITLScheduler::SITLScheduler(SITL_State *sitlState) :
_sitlState(sitlState), _sitlState(sitlState),
stopped_clock_usec(0) stopped_clock_usec(0)
{ {
signal(SIGCONT, sigcont_handler);
pthread_mutex_init(&clock_barrier.m, NULL);
pthread_cond_init(&clock_barrier.cv, NULL);
clock_barrier.state = CLOCK_WAIT_INIT;
} }
void SITLScheduler::init(void *unused) void SITLScheduler::init(void *unused)
@ -92,50 +84,13 @@ uint32_t SITLScheduler::millis()
extern AVR_SITL::SITL_State *g_state; extern AVR_SITL::SITL_State *g_state;
/*
implement a barrier wait for SITL lock-step scheduling. We don't use
pthread_barrier_* as it is not available in cygwin
*/
void SITLScheduler::clock_barrier_wait(void)
{
pthread_mutex_lock(&clock_barrier.m);
while (clock_barrier.state == CLOCK_WAIT_TWO ||
clock_barrier.state == CLOCK_WAIT_THREE) {
pthread_cond_wait(&clock_barrier.cv, &clock_barrier.m);
}
if (clock_barrier.state == CLOCK_WAIT_INIT) {
clock_barrier.state = CLOCK_WAIT_ONE;
pthread_cond_signal(&clock_barrier.cv);
while (clock_barrier.state != CLOCK_WAIT_TWO) {
pthread_cond_wait(&clock_barrier.cv, &clock_barrier.m);
}
clock_barrier.state = CLOCK_WAIT_THREE;
pthread_cond_signal(&clock_barrier.cv);
} else {
clock_barrier.state = CLOCK_WAIT_TWO;
pthread_cond_signal(&clock_barrier.cv);
while (clock_barrier.state != CLOCK_WAIT_THREE) {
pthread_cond_wait(&clock_barrier.cv, &clock_barrier.m);
}
clock_barrier.state = CLOCK_WAIT_INIT;
pthread_cond_signal(&clock_barrier.cv);
}
pthread_mutex_unlock(&clock_barrier.m);
}
void SITLScheduler::delay_microseconds(uint16_t usec) void SITLScheduler::delay_microseconds(uint16_t usec)
{ {
uint64_t start = micros64(); uint64_t start = micros64();
uint64_t dtime; uint64_t dtime;
while ((dtime=(micros64() - start) < usec)) { while ((dtime=(micros64() - start) < usec)) {
if (stopped_clock_usec) { if (stopped_clock_usec) {
/* _sitlState->wait_clock(start+usec);
we are using a synthetic clock. We want to wait until
the stop_clock() call advances the clock
*/
clock_barrier_wait();
} else { } else {
usleep(usec - dtime); usleep(usec - dtime);
} }
@ -317,24 +272,7 @@ void SITLScheduler::panic(const prog_char_t *errormsg) {
*/ */
void SITLScheduler::stop_clock(uint64_t time_usec) void SITLScheduler::stop_clock(uint64_t time_usec)
{ {
if (time_usec == 1) {
// special case for initialisation in synthetic clock mode
stopped_clock_usec = time_usec;
return;
}
if (stopped_clock_usec != 0) {
/*
wait until the main thread is waiting for us. This ensures
that any processing is complete before we advance the clock
*/
clock_barrier_wait();
}
stopped_clock_usec = time_usec; stopped_clock_usec = time_usec;
/*
wait again to ensure the main thread can't get behind the FDM
*/
clock_barrier_wait();
_run_io_procs(false); _run_io_procs(false);
} }

9
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -74,16 +74,9 @@ private:
#endif #endif
void stop_clock(uint64_t time_usec); void stop_clock(uint64_t time_usec);
void clock_barrier_wait();
bool _initialized; bool _initialized;
volatile uint64_t stopped_clock_usec; uint64_t stopped_clock_usec;
enum clock_wait { CLOCK_WAIT_INIT, CLOCK_WAIT_ONE, CLOCK_WAIT_TWO, CLOCK_WAIT_THREE };
struct {
enum clock_wait state;
pthread_mutex_t m;
pthread_cond_t cv;
} clock_barrier;
}; };
#endif #endif
#endif // __AP_HAL_SITL_SCHEDULER_H__ #endif // __AP_HAL_SITL_SCHEDULER_H__