void Rover::startup_INS_ground(void)
{
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// -----------------------
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move vehicle"));
mavlink_delay(1000);
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
AP_InertialSensor::Start_style style;
if (g.skip_gyro_cal) {
style = AP_InertialSensor::WARM_START;
} else {
style = AP_InertialSensor::COLD_START;
}
ins.init(style, ins_sample_rate);
ahrs.reset();
}
void Rover::startup_INS_ground(void)
{
gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC");
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// -----------------------
gcs_send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");
mavlink_delay(1000);
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
}
static void demo_servos(uint8_t i) {
while(i > 0) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
demoing_servos = true;
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
hal.rcout->write(1, 1400);
mavlink_delay(400);
hal.rcout->write(1, 1600);
mavlink_delay(200);
hal.rcout->write(1, 1500);
#endif
demoing_servos = false;
mavlink_delay(400);
i--;
}
}
