Exemple #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);
}
void setup(void) {
 Serial.begin(115200);
  menu();
  I2c.begin();
  I2c.timeOut(20);
  
   if (!compass.init()) {
    Serial.println("compass initialisation failed!");

   while (1) ;
  }
  
  isr_registry.init();
  adc_scheduler.init(&isr_registry);
  APM_RC.Init(&isr_registry);  
  APM_RC.enable_out(CH_1);
  APM_RC.enable_out(CH_2);
  APM_RC.enable_out(CH_3);
  APM_RC.enable_out(CH_4);
  APM_RC.enable_out(CH_5);
  APM_RC.enable_out(CH_6);
  APM_RC.enable_out(CH_7);
  APM_RC.enable_out(CH_8);
  APM_RC.OutputCh(1, valor1);
  APM_RC.OutputCh(2, valor1);
  APM_RC.OutputCh(3, valor1);
  APM_RC.OutputCh(4, valor1);
  APM_RC.OutputCh(5, valor1);
  APM_RC.OutputCh(6, valor1);

DataFlash.Init();
DataFlash.StartWrite(1); 

 imu.init(IMU::COLD_START, delay, flash_leds, &adc_scheduler);
 imu.init_accel(delay, flash_leds);Serial.print("\n");

funcion_serial();
fast_loopTimer=micros();timer1=micros();
  compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD); 
  compass.set_offsets(0,0,0);
  compass.set_declination(ToRad(0.23));
}
/*
  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;
}