mirror of https://github.com/ArduPilot/ardupilot
110 lines
2.8 KiB
Plaintext
110 lines
2.8 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
static void init_tracker()
|
|
{
|
|
hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
|
|
|
|
// gps port
|
|
hal.uartB->begin(38400, 256, 16);
|
|
|
|
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
|
|
"\n\nFree RAM: %u\n"),
|
|
memcheck_available_memory());
|
|
|
|
// Check the EEPROM format version before loading any parameters from EEPROM
|
|
load_parameters();
|
|
|
|
// reset the uartA baud rate after parameter load
|
|
hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD));
|
|
|
|
// init baro before we start the GCS, so that the CLI baro test works
|
|
barometer.init();
|
|
|
|
// init the GCS
|
|
gcs0.init(hal.uartA);
|
|
// 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, 5);
|
|
|
|
// we have a 2nd serial port for telemetry
|
|
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
|
|
128, SERIAL2_BUFSIZE);
|
|
gcs3.init(hal.uartC);
|
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
// Do GPS init
|
|
g_gps = &g_gps_driver;
|
|
|
|
// GPS Initialization
|
|
g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY);
|
|
|
|
mavlink_system.compid = 4;
|
|
mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER;
|
|
|
|
ahrs.init();
|
|
ahrs.set_fly_forward(false);
|
|
|
|
ins.init(AP_InertialSensor::WARM_START, ins_sample_rate);
|
|
ahrs.reset();
|
|
|
|
init_barometer();
|
|
|
|
hal.uartA->set_blocking_writes(false);
|
|
hal.uartC->set_blocking_writes(false);
|
|
|
|
// setup antenna control PWM channels
|
|
channel_yaw.set_angle(4500);
|
|
channel_pitch.set_angle(4500);
|
|
|
|
channel_yaw.output_trim();
|
|
channel_pitch.output_trim();
|
|
|
|
channel_yaw.calc_pwm();
|
|
channel_pitch.calc_pwm();
|
|
|
|
channel_yaw.enable_out();
|
|
channel_pitch.enable_out();
|
|
|
|
gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
|
|
hal.scheduler->delay(1000);
|
|
}
|
|
|
|
// updates the status of the notify objects
|
|
// should be called at 50hz
|
|
static void update_notify()
|
|
{
|
|
notify.update();
|
|
}
|
|
|
|
/*
|
|
* map from a 8 bit EEPROM baud rate to a real baud rate
|
|
*/
|
|
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
|
|
{
|
|
switch (rate) {
|
|
case 1: return 1200;
|
|
case 2: return 2400;
|
|
case 4: return 4800;
|
|
case 9: return 9600;
|
|
case 19: return 19200;
|
|
case 38: return 38400;
|
|
case 57: return 57600;
|
|
case 111: return 111100;
|
|
case 115: return 115200;
|
|
}
|
|
cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
|
|
return default_baud;
|
|
}
|
|
|
|
|