void Plane::startup_INS_ground(void)
{
#if HIL_SUPPORT
if (g.hil_mode == 1) {
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Waiting for first HIL_STATE message"));
hal.scheduler->delay(1000);
}
}
#endif
AP_InertialSensor::Start_style style;
if (g.skip_gyro_cal) {
style = AP_InertialSensor::WARM_START;
arming.set_skip_gyro_cal(true);
} else {
style = AP_InertialSensor::COLD_START;
}
if (style == AP_InertialSensor::COLD_START) {
gcs_send_text_P(MAV_SEVERITY_ALERT, PSTR("Beginning INS calibration; do not move plane"));
hal.scheduler->delay(100);
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_wind_estimation(true);
ins.init(style, ins_sample_rate);
ahrs.reset();
// read Baro pressure at ground
//-----------------------------
init_barometer();
if (airspeed.enabled()) {
// initialize airspeed sensor
// --------------------------
zero_airspeed(true);
} else {
gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("NO airspeed"));
}
}
void Plane::startup_INS_ground(void)
{
#if HIL_SUPPORT
if (g.hil_mode == 1) {
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
hal.scheduler->delay(1000);
}
}
#endif
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane");
hal.scheduler->delay(100);
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_wind_estimation(true);
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
// read Baro pressure at ground
//-----------------------------
init_barometer();
if (airspeed.enabled()) {
// initialize airspeed sensor
// --------------------------
zero_airspeed(true);
} else {
gcs_send_text(MAV_SEVERITY_WARNING,"No airspeed");
}
}
int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv)
{
if (!airspeed.enabled()) {
cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
}else{
print_hit_enter();
zero_airspeed(false);
cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(true);
while(1) {
hal.scheduler->delay(20);
read_airspeed();
cliSerial->printf_P(PSTR("%.1f m/s\n"), (double)airspeed.get_airspeed());
if(cliSerial->available() > 0) {
return (0);
}
}
}
}