mirror of https://github.com/ArduPilot/ardupilot
Copter: changed startup order
this allows BoardConfig to report errors in a way that allows for BRD_TYPE to be changed by the user
This commit is contained in:
parent
80bad445b8
commit
ba7d0d6cd8
|
@ -110,11 +110,20 @@ void Copter::init_ardupilot()
|
|||
// load parameters from EEPROM
|
||||
load_parameters();
|
||||
|
||||
BoardConfig.init();
|
||||
GCS_MAVLINK::set_dataflash(&DataFlash);
|
||||
|
||||
// initialise serial port
|
||||
// initialise serial ports
|
||||
serial_manager.init();
|
||||
|
||||
// setup first port early to allow BoardConfig to report errors
|
||||
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);
|
||||
|
||||
BoardConfig.init();
|
||||
|
||||
// init EPM cargo gripper
|
||||
#if EPM_ENABLED == ENABLED
|
||||
epm.init();
|
||||
|
@ -132,18 +141,13 @@ void Copter::init_ardupilot()
|
|||
|
||||
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();
|
||||
|
||||
// setup telem slots with serial ports
|
||||
for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
|
||||
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
|
||||
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
|
||||
}
|
||||
|
||||
|
@ -159,8 +163,6 @@ void Copter::init_ardupilot()
|
|||
log_init();
|
||||
#endif
|
||||
|
||||
GCS_MAVLINK::set_dataflash(&DataFlash);
|
||||
|
||||
// update motor interlock state
|
||||
update_using_interlock();
|
||||
|
||||
|
|
Loading…
Reference in New Issue