void Plane::init_ardupilot() { // initialise serial port serial_manager.init_console(); cliSerial->printf_P(PSTR("\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(); if (g.hil_mode == 1) { // set sensors to HIL mode ins.set_hil_mode(); compass.set_hil_mode(); barometer.set_hil_mode(); } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 // this must be before BoardConfig.init() so if // BRD_SAFETYENABLE==0 then we don't have safety off yet for (uint8_t tries=0; tries<10; tries++) { if (setup_failsafe_mixing()) { break; } hal.scheduler->delay(10); } #endif BoardConfig.init(); // initialise serial ports serial_manager.init(); // allow servo set on all channels except first 4 ServoRelayEvents.set_channel_mask(0xFFF0); set_control_channels(); // 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(); // initialise rangefinder init_rangefinder(); // initialise battery monitoring battery.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); #if MAVLINK_COMM_NUM_BUFFERS > 2 // setup serial port for telem2 gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); #endif #if MAVLINK_COMM_NUM_BUFFERS > 3 // setup serial port for fourth telemetry port (not used by default) gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); #endif // setup frsky #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 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 apm1_adc.Init(); // APM ADC library initialization #endif // initialise airspeed sensor airspeed.init(); if (g.compass_enabled==true) { if (!compass.init() || !compass.read()) { cliSerial->println_P(PSTR("Compass initialisation failed!")); g.compass_enabled = false; } else { ahrs.set_compass(&compass); } } #if OPTFLOW == ENABLED // make optflow available to libraries ahrs.set_optflow(&optflow); #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); // give AHRS the airspeed sensor ahrs.set_airspeed(&airspeed); // GPS Initialization gps.init(&DataFlash, serial_manager); 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 #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 0); #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 (g.cli_enabled == 1) { const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n"); cliSerial->println_P(msg); if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) { gcs[1].get_uart()->println_P(msg); } if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { gcs[2].get_uart()->println_P(msg); } } #endif // CLI_ENABLED startup_ground(); Log_Write_Startup(TYPE_GROUNDSTART_MSG); // choose the nav controller set_nav_controller(); set_mode((FlightMode)g.initial_mode.get()); // set the correct flight mode // --------------------------- reset_control_switch(); // initialise sensor #if OPTFLOW == ENABLED optflow.init(); #endif }
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(); }
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) { switch(ch_function) { case AUXSW_FLIP: // flip if switch is on, positive throttle and we're actually flying if (ch_flag == AUX_SWITCH_HIGH) { set_mode(FLIP, MODE_REASON_TX_COMMAND); } break; case AUXSW_SIMPLE_MODE: // low = simple mode off, middle or high position turns simple mode on set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; case AUXSW_SUPERSIMPLE_MODE: // low = simple mode off, middle = simple mode, high = super simple mode set_simple_mode(ch_flag); break; case AUXSW_RTL: if (ch_flag == AUX_SWITCH_HIGH) { // engage RTL (if not possible we remain in current flight mode) set_mode(RTL, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in RTL if (control_mode == RTL) { reset_control_switch(); } } break; case AUXSW_SAVE_TRIM: if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) { save_trim(); } break; case AUXSW_SAVE_WP: // save waypoint when switch is brought high if (ch_flag == AUX_SWITCH_HIGH) { // do not allow saving new waypoints while we're in auto or disarmed if (control_mode == AUTO || !motors.armed()) { return; } // do not allow saving the first waypoint with zero throttle if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) { return; } // create new mission command AP_Mission::Mission_Command cmd = {}; // if the mission is empty save a takeoff command if (mission.num_commands() == 0) { // set our location ID to 16, MAV_CMD_NAV_WAYPOINT cmd.id = MAV_CMD_NAV_TAKEOFF; cmd.content.location.options = 0; cmd.p1 = 0; cmd.content.location.lat = 0; cmd.content.location.lng = 0; cmd.content.location.alt = MAX(current_loc.alt,100); // use the current altitude for the target alt for takeoff. // only altitude will matter to the AP mission script for takeoff. if (mission.add_cmd(cmd)) { // log event Log_Write_Event(DATA_SAVEWP_ADD_WP); } } // set new waypoint to current location cmd.content.location = current_loc; // if throttle is above zero, create waypoint command if (channel_throttle->get_control_in() > 0) { cmd.id = MAV_CMD_NAV_WAYPOINT; } else { // with zero throttle, create LAND command cmd.id = MAV_CMD_NAV_LAND; } // save command if (mission.add_cmd(cmd)) { // log event Log_Write_Event(DATA_SAVEWP_ADD_WP); } } break; case AUXSW_CAMERA_TRIGGER: #if CAMERA == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { do_take_picture(); } #endif break; case AUXSW_RANGEFINDER: // enable or disable the rangefinder #if RANGEFINDER_ENABLED == ENABLED if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) { rangefinder_state.enabled = true; } else { rangefinder_state.enabled = false; } #endif break; case AUXSW_FENCE: #if AC_FENCE == ENABLED // enable or disable the fence if (ch_flag == AUX_SWITCH_HIGH) { fence.enable(true); Log_Write_Event(DATA_FENCE_ENABLE); } else { fence.enable(false); Log_Write_Event(DATA_FENCE_DISABLE); } #endif break; case AUXSW_ACRO_TRAINER: switch(ch_flag) { case AUX_SWITCH_LOW: g.acro_trainer = ACRO_TRAINER_DISABLED; Log_Write_Event(DATA_ACRO_TRAINER_DISABLED); break; case AUX_SWITCH_MIDDLE: g.acro_trainer = ACRO_TRAINER_LEVELING; Log_Write_Event(DATA_ACRO_TRAINER_LEVELING); break; case AUX_SWITCH_HIGH: g.acro_trainer = ACRO_TRAINER_LIMITED; Log_Write_Event(DATA_ACRO_TRAINER_LIMITED); break; } break; case AUXSW_GRIPPER: #if GRIPPER_ENABLED == ENABLED switch(ch_flag) { case AUX_SWITCH_LOW: g2.gripper.release(); Log_Write_Event(DATA_GRIPPER_RELEASE); break; case AUX_SWITCH_HIGH: g2.gripper.grab(); Log_Write_Event(DATA_GRIPPER_GRAB); break; } #endif break; case AUXSW_SPRAYER: #if SPRAYER == ENABLED sprayer.run(ch_flag == AUX_SWITCH_HIGH); // if we are disarmed the pilot must want to test the pump sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed()); #endif break; case AUXSW_AUTO: if (ch_flag == AUX_SWITCH_HIGH) { set_mode(AUTO, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in AUTO if (control_mode == AUTO) { reset_control_switch(); } } break; case AUXSW_AUTOTUNE: #if AUTOTUNE_ENABLED == ENABLED // turn on auto tuner switch(ch_flag) { case AUX_SWITCH_LOW: case AUX_SWITCH_MIDDLE: // restore flight mode based on flight mode switch position if (control_mode == AUTOTUNE) { reset_control_switch(); } break; case AUX_SWITCH_HIGH: // start an autotuning session set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND); break; } #endif break; case AUXSW_LAND: if (ch_flag == AUX_SWITCH_HIGH) { set_mode(LAND, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in LAND if (control_mode == LAND) { reset_control_switch(); } } break; case AUXSW_PARACHUTE_ENABLE: #if PARACHUTE == ENABLED // Parachute enable/disable parachute.enabled(ch_flag == AUX_SWITCH_HIGH); #endif break; case AUXSW_PARACHUTE_RELEASE: #if PARACHUTE == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { parachute_manual_release(); } #endif break; case AUXSW_PARACHUTE_3POS: #if PARACHUTE == ENABLED // Parachute disable, enable, release with 3 position switch switch (ch_flag) { case AUX_SWITCH_LOW: parachute.enabled(false); Log_Write_Event(DATA_PARACHUTE_DISABLED); break; case AUX_SWITCH_MIDDLE: parachute.enabled(true); Log_Write_Event(DATA_PARACHUTE_ENABLED); break; case AUX_SWITCH_HIGH: parachute.enabled(true); parachute_manual_release(); break; } #endif break; case AUXSW_MISSION_RESET: if (ch_flag == AUX_SWITCH_HIGH) { mission.reset(); } break; case AUXSW_ATTCON_FEEDFWD: // enable or disable feed forward attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_ATTCON_ACCEL_LIM: // enable or disable accel limiting by restoring defaults attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RETRACT_MOUNT: #if MOUNT == ENABLE switch (ch_flag) { case AUX_SWITCH_HIGH: camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT); break; case AUX_SWITCH_LOW: camera_mount.set_mode_to_default(); break; } #endif break; case AUXSW_RELAY: ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RELAY2: ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RELAY3: ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_RELAY4: ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_LANDING_GEAR: switch (ch_flag) { case AUX_SWITCH_LOW: landinggear.set_cmd_mode(LandingGear_Deploy); break; case AUX_SWITCH_MIDDLE: landinggear.set_cmd_mode(LandingGear_Auto); break; case AUX_SWITCH_HIGH: landinggear.set_cmd_mode(LandingGear_Retract); break; } break; case AUXSW_LOST_COPTER_SOUND: switch (ch_flag) { case AUX_SWITCH_HIGH: AP_Notify::flags.vehicle_lost = true; break; case AUX_SWITCH_LOW: AP_Notify::flags.vehicle_lost = false; break; } break; case AUXSW_MOTOR_ESTOP: // Turn on Emergency Stop logic when channel is high set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH); break; case AUXSW_MOTOR_INTERLOCK: // Turn on when above LOW, because channel will also be used for speed // control signal in tradheli ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; case AUXSW_BRAKE: // brake flight mode if (ch_flag == AUX_SWITCH_HIGH) { set_mode(BRAKE, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in BRAKE if (control_mode == BRAKE) { reset_control_switch(); } } break; case AUXSW_THROW: // throw flight mode if (ch_flag == AUX_SWITCH_HIGH) { set_mode(THROW, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in throw mode if (control_mode == THROW) { reset_control_switch(); } } break; case AUXSW_AVOID_ADSB: // enable or disable AP_Avoidance if (ch_flag == AUX_SWITCH_HIGH) { avoidance_adsb.enable(); Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE); } else { avoidance_adsb.disable(); Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE); } break; case AUXSW_PRECISION_LOITER: #if PRECISION_LANDING == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: set_precision_loiter_enabled(true); break; case AUX_SWITCH_LOW: set_precision_loiter_enabled(false); break; } #endif break; case AUXSW_AVOID_PROXIMITY: #if PROXIMITY_ENABLED == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: avoid.proximity_avoidance_enable(true); Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE); break; case AUX_SWITCH_LOW: avoid.proximity_avoidance_enable(false); Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE); break; } #endif break; } }
void Plane::init_ardupilot() { // initialise serial port serial_manager.init_console(); cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", (unsigned)hal.util->available_memory()); // // Check the EEPROM format version before loading any parameters from EEPROM // load_parameters(); #if HIL_SUPPORT if (g.hil_mode == 1) { // set sensors to HIL mode ins.set_hil_mode(); compass.set_hil_mode(); barometer.set_hil_mode(); } #endif set_control_channels(); init_rc_out_main(); #if HAVE_PX4_MIXER if (!quadplane.enable) { // this must be before BoardConfig.init() so if // BRD_SAFETYENABLE==0 then we don't have safety off yet. For // quadplanes we wait till AP_Motors is initialised for (uint8_t tries=0; tries<10; tries++) { if (setup_failsafe_mixing()) { break; } hal.scheduler->delay(10); } } #endif GCS_MAVLINK::set_dataflash(&DataFlash); // initialise serial ports serial_manager.init(); gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); // 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); // setup any board specific drivers BoardConfig.init(); // allow servo set on all channels except first 4 ServoRelayEvents.set_channel_mask(0xFFF0); // 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(); // initialise rangefinder init_rangefinder(); // initialise battery monitoring battery.init(); rpm_sensor.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 = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) { gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); } // setup frsky #if FRSKY_TELEM_ENABLED == ENABLED // setup frsky, and pass a number of parameters to the library frsky_telemetry.init(serial_manager, FIRMWARE_STRING, MAV_TYPE_FIXED_WING, &g.fs_batt_voltage, &g.fs_batt_mah); #endif mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif // initialise airspeed sensor airspeed.init(); if (g.compass_enabled==true) { bool compass_ok = compass.init() && compass.read(); #if HIL_SUPPORT if (g.hil_mode != 0) { compass_ok = true; } #endif if (!compass_ok) { cliSerial->println("Compass initialisation failed!"); g.compass_enabled = false; } else { ahrs.set_compass(&compass); } } #if OPTFLOW == ENABLED // make optflow available to libraries if (optflow.enabled()) { ahrs.set_optflow(&optflow); } #endif // give AHRS the airspeed sensor ahrs.set_airspeed(&airspeed); // GPS Initialization gps.init(&DataFlash, serial_manager); init_rc_in(); // sets up rc channels from radio relay.init(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(&DataFlash, serial_manager); #endif #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 0); #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 (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 // CLI_ENABLED init_capabilities(); quadplane.setup(); startup_ground(); // don't initialise aux rc output until after quadplane is setup as // that can change initial values of channels init_rc_out_aux(); // choose the nav controller set_nav_controller(); set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN); // set the correct flight mode // --------------------------- reset_control_switch(); // initialise sensor #if OPTFLOW == ENABLED if (optflow.enabled()) { optflow.init(); } #endif }