void ReplayVehicle::setup(void)
{
load_parameters();
// we pass zero log structures, as we will be outputting the log
// structures we need manually, to prevent FMT duplicates
dataflash.Init(log_structure, 0);
dataflash.StartNewLog();
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true);
EKF2.set_enable(true);
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);
barometer.update();
compass.init();
ins.set_hil_mode();
}
void loop()
{
// run accumulate() at 50Hz and update() at 10Hz
if((hal.scheduler->micros() - timer) > 20*1000UL) {
timer = hal.scheduler->micros();
barometer.accumulate();
if (counter++ < 5) {
return;
}
counter = 0;
barometer.update();
uint32_t read_time = hal.scheduler->micros() - timer;
float alt = barometer.get_altitude();
if (!barometer.healthy()) {
hal.console->println("not healthy");
return;
}
hal.console->print("Pressure:");
hal.console->print(barometer.get_pressure());
hal.console->print(" Temperature:");
hal.console->print(barometer.get_temperature());
hal.console->print(" Altitude:");
hal.console->print(alt);
hal.console->printf(" climb=%.2f t=%u",
barometer.get_climb_rate(),
(unsigned)read_time);
hal.console->println();
} else {
hal.scheduler->delay(1);
}
}
void ReplayVehicle::setup(void) {
dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
dataflash.StartNewLog();
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true);
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);
barometer.update();
compass.init();
ins.set_hil_mode();
}
