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();
}
}
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;
}