Example #1
0
/*
  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;
}