int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv) { cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n")); print_hit_enter(); init_barometer(); while(1) { hal.scheduler->delay(100); barometer.update(); if (!barometer.healthy()) { cliSerial->println_P(PSTR("not healthy")); } else { cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"), (double)barometer.get_altitude(), (double)barometer.get_pressure(), (double)barometer.get_temperature()); } if(cliSerial->available() > 0) { return (0); } } }
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 Sub::test_baro(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); init_barometer(true); while(1) { delay(100); read_barometer(); if (!barometer.healthy()) { cliSerial->println("not healthy"); } else { cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", (double)(baro_alt / 100.0f), (double)barometer.get_pressure(), (double)barometer.get_temperature()); } if(cliSerial->available() > 0) { return (0); } } return 0; }
void Copter::init_ardupilot() { if (!hal.gpio->usb_connected()) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at // least one second after powering up. Simplest solution for // now is to delay for 1 second. Something more elegant may be // added later delay(1000); } // initialise serial port serial_manager.init_console(); // init vehicle capabilties init_capabilities(); cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", (unsigned)hal.util->available_memory()); // // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) // report_version(); // load parameters from EEPROM load_parameters(); BoardConfig.init(); // initialise serial port serial_manager.init(); // init EPM cargo gripper #if EPM_ENABLED == ENABLED epm.init(); #endif // initialise notify system // disable external leds if epm is enabled because of pin conflict on the APM notify.init(true); // initialise battery monitor battery.init(); // Init RSSI rssi.init(); barometer.init(); // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to // hal.scheduler->delay. hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. ap.usb_connected = true; check_usb_mux(); // init the GCS connected to the console gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); // init telemetry port gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); // setup serial port for telem2 gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); // setup serial port for fourth telemetry port (not used by default) gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); #if FRSKY_TELEM_ENABLED == ENABLED // setup frsky frsky_telemetry.init(serial_manager); #endif // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif GCS_MAVLINK::set_dataflash(&DataFlash); // update motor interlock state update_using_interlock(); #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init(); #endif init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs // initialise which outputs Servo and Relay events can use ServoRelayEvents.set_channel_mask(~motors.get_motor_mask()); relay.init(); /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); // Do GPS init gps.init(&DataFlash, serial_manager); if(g.compass_enabled) init_compass(); #if OPTFLOW == ENABLED // make optflow available to AHRS ahrs.set_optflow(&optflow); #endif // init Location class Location_Class::set_ahrs(&ahrs); #if AP_TERRAIN_AVAILABLE && AC_TERRAIN Location_Class::set_terrain(&terrain); wp_nav.set_terrain(&terrain); #endif pos_control.set_dt(MAIN_LOOP_SECONDS); // init the optical flow sensor init_optflow(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(&DataFlash, serial_manager); #endif #if PRECISION_LANDING == ENABLED // initialise precision landing init_precland(); #endif #ifdef USERHOOK_INIT USERHOOK_INIT #endif #if CLI_ENABLED == ENABLED if (g.cli_enabled) { const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; cliSerial->println(msg); if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) { gcs[1].get_uart()->println(msg); } if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { gcs[2].get_uart()->println(msg); } } #endif // CLI_ENABLED #if HIL_MODE != HIL_MODE_DISABLED 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"); delay(1000); } // set INS to HIL mode ins.set_hil_mode(); #endif // read Baro pressure at ground //----------------------------- init_barometer(true); // initialise sonar #if CONFIG_SONAR == ENABLED init_sonar(); #endif // initialise AP_RPM library rpm_sensor.init(); // initialise mission library mission.init(); // initialise the flight mode and aux switch // --------------------------- reset_control_switch(); init_aux_switches(); startup_INS_ground(); // set landed flags set_land_complete(true); set_land_complete_maybe(true); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly serial_manager.set_blocking_writes_all(false); // enable CPU failsafe failsafe_enable(); ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); cliSerial->print("\nReady to FLY "); // flag that initialisation has completed ap.initialised = true; }
void Rover::init_ardupilot() { // initialise console serial port serial_manager.init_console(); cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", hal.util->available_memory()); // // Check the EEPROM format version before loading any parameters from EEPROM. // load_parameters(); BoardConfig.init(); // initialise serial ports serial_manager.init(); ServoRelayEvents.set_channel_mask(0xFFF0); set_control_channels(); battery.init(); // keep a record of how many resets have happened. This can be // used to detect in-flight resets g.num_resets.set_and_save(g.num_resets+1); // init baro before we start the GCS, so that the CLI baro test works barometer.init(); // init the GCS gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. usb_connected = true; check_usb_mux(); // setup serial port for telem1 gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); // setup serial port for telem2 gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); // setup serial port for fourth telemetry port (not used by default) gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); // setup frsky telemetry #if FRSKY_TELEM_ENABLED == ENABLED frsky_telemetry.init(serial_manager); #endif mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); if (g.compass_enabled==true) { if (!compass.init()|| !compass.read()) { cliSerial->println("Compass initialisation failed!"); g.compass_enabled = false; } else { ahrs.set_compass(&compass); //compass.get_offsets(); // load offsets to account for airframe magnetic interference } } // initialise sonar init_sonar(); // and baro for EKF init_barometer(); // Do GPS init gps.init(&DataFlash, serial_manager); rc_override_active = hal.rcin->set_overrides(rc_override, 8); init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs relay.init(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(serial_manager); #endif /* setup the 'main loop is dead' check. Note that this relies on the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); #if CLI_ENABLED == ENABLED // If the switch is in 'menu' mode, run the main menu. // // Since we can't be sure that the setup or test mode won't leave // the system in an odd state, we don't let the user exit the top // menu; they must reset in order to fly. // if (g.cli_enabled == 1) { const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; cliSerial->println(msg); if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) { gcs[1].get_uart()->println(msg); } if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { gcs[2].get_uart()->println(msg); } } #endif init_capabilities(); startup_ground(); set_mode((enum mode)g.initial_mode.get()); // set the correct flight mode // --------------------------- reset_control_switch(); }
void Tracker::init_tracker() { // initialise console serial port serial_manager.init_console(); hal.console->printf("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n", hal.util->available_memory()); // Check the EEPROM format version before loading any parameters from EEPROM load_parameters(); BoardConfig.init(); // initialise serial ports serial_manager.init(); // init baro before we start the GCS, so that the CLI baro test works barometer.init(); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. usb_connected = true; check_usb_mux(); // setup telem slots with serial ports for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) { gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); gcs[i].set_snoop(mavlink_snoop_static); } // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif GCS_MAVLINK::set_dataflash(&DataFlash); if (g.compass_enabled==true) { if (!compass.init() || !compass.read()) { hal.console->println("Compass initialisation failed!"); g.compass_enabled = false; } else { ahrs.set_compass(&compass); } } // GPS Initialization gps.init(NULL, serial_manager); ahrs.init(); ahrs.set_fly_forward(false); ins.init(scheduler.get_loop_rate_hz()); ahrs.reset(); init_barometer(true); // set serial ports non-blocking serial_manager.set_blocking_writes_all(false); // initialise servos init_servos(); // use given start positions - useful for indoor testing, and // while waiting for GPS lock // sanity check location if (fabsf(g.start_latitude) <= 90.0f && fabsf(g.start_longitude) <= 180.0f) { current_loc.lat = g.start_latitude * 1.0e7f; current_loc.lng = g.start_longitude * 1.0e7f; gcs_send_text(MAV_SEVERITY_NOTICE, "Ignoring invalid START_LATITUDE or START_LONGITUDE parameter"); } // see if EEPROM has a default location as well if (current_loc.lat == 0 && current_loc.lng == 0) { get_home_eeprom(current_loc); } init_capabilities(); gcs_send_text(MAV_SEVERITY_INFO,"Ready to track"); hal.scheduler->delay(1000); // Why???? set_mode(AUTO); // tracking if (g.startup_delay > 0) { // arm servos with trim value to allow them to start up (required // for some servos) prepare_servos(); } }
/** * Main system entry point */ int main (void) { SystemInit(); /* Initialise Pins */ CUTDOWN_OFF(); HEATER_OFF(); MBED_OFF(); GREEN_OFF(); /* Update the value of SystemCoreClock */ SystemCoreClockUpdate(); /* Initialise Interfaces */ i2c_init(); spi_init(process_imu_frame); // IMU sd_spi_init(); // SD uart_init(); // GPS pwrmon_init(); // ADC /* Initialise Sensors */ init_barometer(); /* SD Card */ if (initialise_card()) { // Initialised to something if (disk_initialize() == 0) { // Disk initialisation was successful sd_good = 1; } } GREEN_ON(); /* Configure the SysTick */ NVIC_SetPriority(SysTick_IRQn, 0); // Highest Priority Interrupt SysTick_Config(SystemCoreClock / RTTY_BAUD); /* Watchdog - Disabled for debugging */ #ifndef WATCHDOG_DISABLED init_watchdog(); #endif struct barometer* b; struct imu_raw ir; struct gps_data gd; struct gps_time gt; double alt, ext_temp; int tx_length; // The length of the built tx string char tx_string[TX_STRING_LENGTH]; while (1) { /* Grab Data */ pwrmon_start(pwrmon_callback); b = get_barometer(); get_imu_raw_data(&ir); get_gps_data(&gd); get_gps_time(>); ext_temp = get_temperature(); /* Data Processing */ if (b->valid) { alt = pressure_to_altitude(b->pressure); } else { alt = -1; b->temperature = -1; } /* Act on the data */ control_gsm(alt); control_cutdown(ticks_until_cutdown, alt); control_heater(b->temperature); /* Create a protocol string */ int cutstat; if (ticks_until_cutdown == 0) { cutstat = -1; } else { cutstat = ticks_until_cutdown / (RTTY_BAUD*60); } tx_length = build_communications_frame(tx_string, TX_STRING_LENGTH, >, b, &gd, alt, ext_temp, &ir, cutstat, cutdown_voltage); /* Transmit - Quietly fails if another transmission is ongoing */ rtty_set_string(tx_string, tx_length); /* Store */ if (sd_good) { tx_length -= 2; // Remove \n\0 tx_length += communications_frame_add_extra(tx_string + tx_length, TX_STRING_LENGTH - tx_length, &ir); disk_write_next_block((uint8_t*)tx_string, tx_length+1); // Include null terminator } /* Housekeeping */ GREEN_TOGGLE(); feed_watchdog(); } }
void Tracker::init_tracker() { // initialise console serial port serial_manager.init_console(); hal.console->printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n"), hal.util->available_memory()); // Check the EEPROM format version before loading any parameters from EEPROM load_parameters(); BoardConfig.init(); // initialise serial ports serial_manager.init(); // init baro before we start the GCS, so that the CLI baro test works barometer.init(); // init the GCS and start snooping for vehicle data gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); gcs[0].set_snoop(mavlink_snoop_static); // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. usb_connected = true; check_usb_mux(); // setup serial port for telem1 and start snooping for vehicle data gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); gcs[1].set_snoop(mavlink_snoop_static); #if MAVLINK_COMM_NUM_BUFFERS > 2 // setup serial port for telem2 and start snooping for vehicle data gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); gcs[2].set_snoop(mavlink_snoop_static); #endif #if MAVLINK_COMM_NUM_BUFFERS > 3 // setup serial port for fourth telemetry port (not used by default) and start snooping for vehicle data gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); gcs[3].set_snoop(mavlink_snoop_static); #endif mavlink_system.sysid = g.sysid_this_mav; if (g.compass_enabled==true) { if (!compass.init() || !compass.read()) { hal.console->println_P(PSTR("Compass initialisation failed!")); g.compass_enabled = false; } else { ahrs.set_compass(&compass); } } // GPS Initialization gps.init(NULL, serial_manager); ahrs.init(); ahrs.set_fly_forward(false); ins.init(AP_InertialSensor::WARM_START, ins_sample_rate); ahrs.reset(); init_barometer(); // set serial ports non-blocking serial_manager.set_blocking_writes_all(false); // initialise servos init_servos(); // use given start positions - useful for indoor testing, and // while waiting for GPS lock current_loc.lat = g.start_latitude * 1.0e7f; current_loc.lng = g.start_longitude * 1.0e7f; // see if EEPROM has a default location as well if (current_loc.lat == 0 && current_loc.lng == 0) { get_home_eeprom(current_loc); } gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track.")); hal.scheduler->delay(1000); // Why???? set_mode(AUTO); // tracking if (g.startup_delay > 0) { // arm servos with trim value to allow them to start up (required // for some servos) prepare_servos(); } // calibrate pressure on startup by default nav_status.need_altitude_calibration = true; }
void Sub::init_ardupilot() { if (!hal.gpio->usb_connected()) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at // least one second after powering up. Simplest solution for // now is to delay for 1 second. Something more elegant may be // added later hal.scheduler->delay(1000); } // initialise serial port serial_manager.init_console(); cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", (unsigned)hal.util->available_memory()); // // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) // report_version(); // load parameters from EEPROM load_parameters(); BoardConfig.init(); // initialise serial port serial_manager.init(); // init cargo gripper #if GRIPPER_ENABLED == ENABLED g2.gripper.init(); #endif // initialise notify system notify.init(true); // initialise battery monitor battery.init(); barometer.init(); celsius.init(); // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to // hal.scheduler->delay. hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. ap.usb_connected = true; check_usb_mux(); // setup telem slots with serial ports for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) { gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); } // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif gcs().set_dataflash(&DataFlash); init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs init_joystick(); // joystick initialization // initialise which outputs Servo and Relay events can use ServoRelayEvents.set_channel_mask(~motors.get_motor_mask()); relay.init(); /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); // Do GPS init gps.init(&DataFlash, serial_manager); if (g.compass_enabled) { init_compass(); } #if OPTFLOW == ENABLED // make optflow available to AHRS ahrs.set_optflow(&optflow); #endif // init Location class Location_Class::set_ahrs(&ahrs); #if AP_TERRAIN_AVAILABLE && AC_TERRAIN Location_Class::set_terrain(&terrain); wp_nav.set_terrain(&terrain); #endif #if AVOIDANCE_ENABLED == ENABLED wp_nav.set_avoidance(&avoid); #endif pos_control.set_dt(MAIN_LOOP_SECONDS); // init the optical flow sensor init_optflow(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(&DataFlash, serial_manager); #endif #ifdef USERHOOK_INIT USERHOOK_INIT #endif #if CLI_ENABLED == ENABLED if (g.cli_enabled) { const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; cliSerial->println(msg); if (gcs_chan[1].initialised && (gcs_chan[1].get_uart() != NULL)) { gcs_chan[1].get_uart()->println(msg); } if (num_gcs > 2 && gcs_chan[2].initialised && (gcs_chan[2].get_uart() != NULL)) { gcs_chan[2].get_uart()->println(msg); } } #endif // CLI_ENABLED #if HIL_MODE != HIL_MODE_DISABLED 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); } // set INS to HIL mode ins.set_hil_mode(); #endif // read Baro pressure at ground //----------------------------- init_barometer(false); barometer.update(); for (uint8_t i = 0; i < barometer.num_instances(); i++) { if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER && barometer.healthy(i)) { barometer.set_primary_baro(i); ap.depth_sensor_present = true; break; } } if (!ap.depth_sensor_present) { // We only have onboard baro // No external underwater depth sensor detected barometer.set_primary_baro(0); EKF2.set_baro_alt_noise(10.0f); // Readings won't correspond with rest of INS EKF3.set_baro_alt_noise(10.0f); } else { EKF2.set_baro_alt_noise(0.1f); EKF3.set_baro_alt_noise(0.1f); } leak_detector.init(); // backwards compatibility if (attitude_control.get_accel_yaw_max() < 110000.0f) { attitude_control.save_accel_yaw_max(110000.0f); } last_pilot_heading = ahrs.yaw_sensor; // initialise rangefinder #if RANGEFINDER_ENABLED == ENABLED init_rangefinder(); #endif // initialise AP_RPM library #if RPM_ENABLED == ENABLED rpm_sensor.init(); #endif // initialise mission library mission.init(); startup_INS_ground(); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly serial_manager.set_blocking_writes_all(false); // enable CPU failsafe failsafe_enable(); ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); // init vehicle capabilties init_capabilities(); cliSerial->print("\nReady to FLY "); // flag that initialisation has completed ap.initialised = true; }