/*
setup for SITL handling
*/
void sitl_setup(void)
{
parent_pid = getppid();
rcout_addr.sin_family = AF_INET;
rcout_addr.sin_port = htons(RCOUT_PORT);
inet_pton(AF_INET, "127.0.0.1", &rcout_addr.sin_addr);
setup_timer();
setup_fdm();
sitl_setup_adc();
printf("Starting SITL input\n");
// setup some initial values
sitl_update_barometer(desktop_state.initial_height);
sitl_update_adc(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0);
sitl_update_compass(0, 0, 0, 0);
sitl_update_gps(0, 0, 0, 0, 0, false);
}
/*
timer called at 1kHz
*/
static void timer_handler(int signum)
{
static uint32_t last_update_count;
/* make sure we die if our parent dies */
if (kill(parent_pid, 0) != 0) {
exit(1);
}
/* check for packet from flight sim */
sitl_fdm_input();
// trigger all timers
timer_scheduler.run();
// trigger RC input
if (isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]) {
isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]();
}
// send RC output to flight sim
sitl_simulator_output();
if (update_count == 0) {
sitl_update_gps(0, 0, 0, 0, 0, false);
return;
}
if (update_count == last_update_count) {
return;
}
last_update_count = update_count;
sitl_update_gps(sim_state.latitude, sim_state.longitude,
sim_state.altitude,
sim_state.speedN, sim_state.speedE, true);
sitl_update_adc(sim_state.rollDeg, sim_state.pitchDeg, sim_state.yawDeg,
sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate,
sim_state.xAccel, sim_state.yAccel, sim_state.zAccel,
sim_state.airspeed);
sitl_update_barometer(sim_state.altitude);
sitl_update_compass(sim_state.heading, sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading);
}
/*
timer called at 1kHz
*/
static void timer_handler(int signum)
{
static uint32_t last_update_count;
static bool running;
if (running) {
return;
}
running = true;
cli();
#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 (millis() - last_report > 1000) {
printf("TH %u cps\n", count);
count = 0;
last_report = millis();
}
#endif
/* check for packet from flight sim */
sitl_fdm_input();
// trigger all timers
timer_scheduler.run();
// trigger RC input
if (isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]) {
isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]();
}
// send RC output to flight sim
sitl_simulator_output();
if (update_count == 0) {
sitl_update_gps(0, 0, 0, 0, 0, false);
sei();
running = false;
return;
}
if (update_count == last_update_count) {
sei();
running = false;
return;
}
last_update_count = update_count;
sitl_update_gps(sim_state.latitude, sim_state.longitude,
sim_state.altitude,
sim_state.speedN, sim_state.speedE, true);
sitl_update_adc(sim_state.rollDeg, sim_state.pitchDeg, sim_state.yawDeg,
sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate,
sim_state.xAccel, sim_state.yAccel, sim_state.zAccel,
sim_state.airspeed);
sitl_update_barometer(sim_state.altitude);
sitl_update_compass(sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading);
sei();
running = false;
}