/*
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;
}
