2011-04-16 17:44:44 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*****************************************************************************
2012-08-21 23:19:50 -03:00
* The init_ardupilot function processes everything we need for an in - air restart
* We will determine later if we are actually on the ground and process a
* ground start in that case.
*
2011-04-16 17:44:44 -03:00
*****************************************************************************/
2011-07-17 07:34:05 -03:00
#if CLI_ENABLED == ENABLED
2011-04-16 17:44:44 -03:00
// Functions called from the top-level menu
2012-08-21 23:19:50 -03:00
static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
2012-11-24 03:20:03 -04:00
static int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
2011-04-16 17:44:44 -03:00
// This is the help function
2012-08-21 23:19:50 -03:00
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
2011-04-16 17:44:44 -03:00
{
2012-11-21 02:08:03 -04:00
cliSerial->printf_P(PSTR("Commands:\n"
2012-08-21 23:19:50 -03:00
" logs\n"
" setup\n"
" test\n"
2012-11-24 03:20:03 -04:00
" reboot\n"
2012-08-21 23:19:50 -03:00
"\n"));
return(0);
2011-04-16 17:44:44 -03:00
}
// Command/function table for the top-level menu.
const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
2012-08-21 23:19:50 -03:00
{"logs", process_logs},
{"setup", setup_mode},
{"test", test_mode},
2012-11-24 03:20:03 -04:00
{"reboot", reboot_board},
2012-08-21 23:19:50 -03:00
{"help", main_menu_help},
2011-04-16 17:44:44 -03:00
};
// Create the top-level menu object.
2011-09-17 15:25:31 -03:00
MENU(main_menu, THISFIRMWARE, main_menu_commands);
2011-04-16 17:44:44 -03:00
2012-11-24 03:20:03 -04:00
static int8_t reboot_board(uint8_t argc, const Menu::arg *argv)
{
2013-09-03 22:59:02 -03:00
hal.scheduler->reboot(false);
2012-11-24 03:20:03 -04:00
return 0;
}
2011-10-27 04:35:25 -03:00
// the user wants the CLI. It never exits
2012-12-12 19:46:20 -04:00
static void run_cli(AP_HAL::UARTDriver *port)
2011-10-27 04:35:25 -03:00
{
2012-11-21 02:08:03 -04:00
cliSerial = port;
Menu::set_port(port);
port->set_blocking_writes(true);
2012-12-23 17:51:33 -04:00
// disable the mavlink delay callback
hal.scheduler->register_delay_callback(NULL, 5);
2013-03-02 04:54:18 -04:00
// disable main_loop failsafe
failsafe_disable();
2013-03-21 00:07:00 -03:00
// cut the engines
if(motors.armed()) {
motors.armed(false);
motors.output();
}
2013-10-29 01:28:27 -03:00
2011-10-27 04:35:25 -03:00
while (1) {
main_menu.run();
}
}
2011-07-17 07:34:05 -03:00
#endif // CLI_ENABLED
2011-07-17 07:32:00 -03:00
static void init_ardupilot()
2011-04-16 17:44:44 -03:00
{
2013-09-20 20:21:17 -03:00
if (!hal.gpio->usb_connected()) {
2011-11-20 05:42:51 -04:00
// 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);
}
2012-08-21 23:19:50 -03:00
// Console serial port
//
// The console port buffers are defined to be sufficiently large to support
2012-03-30 02:53:50 -03:00
// the MAVLink protocol efficiently
//
2013-01-10 19:31:55 -04:00
#if HIL_MODE != HIL_MODE_DISABLED
// we need more memory for HIL, as we get a much higher packet rate
hal.uartA->begin(SERIAL0_BAUD, 256, 256);
#else
// use a bit less for non-HIL operation
2013-10-08 08:05:19 -03:00
hal.uartA->begin(SERIAL0_BAUD, 512, 128);
2013-01-10 19:31:55 -04:00
#endif
2011-04-16 17:44:44 -03:00
2012-08-21 23:19:50 -03:00
// GPS serial port.
//
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
2012-06-08 03:39:48 -03:00
// standard gps running. Note that we need a 256 byte buffer for some
// GPS types (eg. UBLOX)
2012-12-13 15:48:01 -04:00
hal.uartB->begin(38400, 256, 16);
2012-08-21 23:19:50 -03:00
#endif
2011-04-16 17:44:44 -03:00
2013-11-08 07:29:29 -04:00
cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
2012-08-21 23:19:50 -03:00
"\n\nFree RAM: %u\n"),
2013-12-28 01:02:32 -04:00
hal.util->available_memory());
2011-04-16 17:44:44 -03:00
2013-10-12 04:24:00 -03:00
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
/*
run the timer a bit slower on APM2 to reduce the interrupt load
on the CPU
*/
hal.scheduler->set_timer_speed(500);
#endif
2012-08-21 23:19:50 -03:00
//
2012-09-13 09:49:11 -03:00
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
2012-08-21 23:19:50 -03:00
//
report_version();
2011-04-16 17:44:44 -03:00
2012-02-12 07:27:51 -04:00
// load parameters from EEPROM
load_parameters();
2011-04-16 17:44:44 -03:00
2014-01-19 21:58:12 -04:00
BoardConfig.init();
2014-01-20 01:05:31 -04:00
// FIX: this needs to be the inverse motors mask
ServoRelayEvents.set_channel_mask(0xFFF0);
2013-12-19 01:12:19 -04:00
relay.init();
bool enable_external_leds = true;
// init EPM cargo gripper
#if EPM_ENABLED == ENABLED
epm.init();
enable_external_leds = !epm.enabled();
#endif
// initialise notify system
// disable external leds if epm is enabled because of pin conflict on the APM
notify.init(enable_external_leds);
// initialise battery monitor
battery.init();
#if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
sonar_analog_source = new AP_ADC_AnalogSource(
&adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
sonar_analog_source = hal.analogin->channel(
CONFIG_SONAR_SOURCE_ANALOG_PIN);
#else
#warning "Invalid CONFIG_SONAR_SOURCE"
#endif
sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
&sonar_mode_filter);
#endif
rssi_analog_source = hal.analogin->channel(g.rssi_pin);
2013-05-25 00:21:29 -03:00
#if HIL_MODE != HIL_MODE_ATTITUDE
barometer.init();
#endif
2012-08-21 23:19:50 -03:00
// init the GCS
2013-11-23 06:45:42 -04:00
gcs[0].init(hal.uartA);
2011-11-20 05:42:51 -04:00
2012-12-13 19:27:42 -04:00
// 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, 5);
2013-09-20 20:21:17 -03:00
// we start by assuming USB connected, as we initialed the serial
2013-10-29 01:28:27 -03:00
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
2013-10-08 03:25:14 -03:00
ap.usb_connected = true;
2013-09-20 20:21:17 -03:00
check_usb_mux();
2013-09-19 03:24:23 -03:00
#if CONFIG_HAL_BOARD != HAL_BOARD_APM2
// we have a 2nd serial port for telemetry on all boards except
// APM2. We actually do have one on APM2 but it isn't necessary as
2013-10-29 01:28:27 -03:00
// a MUX is used
2013-11-25 19:58:50 -04:00
hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD), 128, 128);
2013-11-23 06:45:42 -04:00
gcs[1].init(hal.uartC);
2011-11-20 05:42:51 -04:00
#endif
2013-11-25 19:58:50 -04:00
#if MAVLINK_COMM_NUM_BUFFERS > 2
2013-11-23 06:45:42 -04:00
if (hal.uartD != NULL) {
2013-11-25 19:58:50 -04:00
hal.uartD->begin(map_baudrate(g.serial2_baud, SERIAL2_BAUD), 128, 128);
2013-11-23 06:45:42 -04:00
gcs[2].init(hal.uartD);
}
2013-11-25 19:58:50 -04:00
#endif
2011-08-01 08:39:17 -03:00
2011-12-02 16:54:36 -04:00
// identify ourselves correctly with the ground station
2012-08-21 23:19:50 -03:00
mavlink_system.sysid = g.sysid_this_mav;
2012-02-02 18:53:08 -04:00
mavlink_system.type = 2; //MAV_QUADROTOR;
2012-02-11 02:25:24 -04:00
2011-12-28 00:53:05 -04:00
#if LOGGING_ENABLED == ENABLED
2013-12-16 20:32:50 -04:00
DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
2011-12-28 00:53:05 -04:00
if (!DataFlash.CardInserted()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash inserted"));
g.log_bitmask.set(0);
} else if (DataFlash.NeedErase()) {
2011-12-17 19:19:41 -04:00
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
2012-08-21 23:19:50 -03:00
do_erase_logs();
2013-11-23 06:45:42 -04:00
gcs[0].reset_cli_timeout();
2012-08-21 23:19:50 -03:00
}
2011-12-28 00:53:05 -04:00
#endif
2011-12-17 19:19:41 -04:00
2012-08-21 23:19:50 -03:00
init_rc_in(); // sets up rc channels from radio
2013-05-16 04:32:00 -03:00
init_rc_out(); // sets up motors and output to escs
2012-10-09 00:30:17 -03:00
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
2012-12-13 15:48:01 -04:00
hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
2012-06-29 08:51:05 -03:00
2011-11-12 22:47:54 -04:00
#if HIL_MODE != HIL_MODE_ATTITUDE
2012-08-21 23:19:50 -03:00
#if CONFIG_ADC == ENABLED
// begin filtering the ADC Gyros
2012-12-13 15:48:01 -04:00
adc.Init(); // APM ADC library initialization
2012-08-21 23:19:50 -03:00
#endif // CONFIG_ADC
2011-11-12 23:48:32 -04:00
#endif // HIL_MODE
2011-04-16 17:44:44 -03:00
2012-08-21 23:19:50 -03:00
// Do GPS init
g_gps = &g_gps_driver;
2012-06-10 03:36:18 -03:00
// GPS Initialization
2012-12-17 22:36:57 -04:00
g_gps->init(hal.uartB, GPS::GPS_ENGINE_AIRBORNE_1G);
2011-04-16 17:44:44 -03:00
2012-08-21 23:19:50 -03:00
if(g.compass_enabled)
init_compass();
2011-04-16 17:44:44 -03:00
2013-12-29 09:09:42 -04:00
// initialise attitude and position controllers
2013-12-16 23:25:33 -04:00
attitude_control.set_dt(MAIN_LOOP_SECONDS);
2013-12-29 09:09:42 -04:00
pos_control.set_dt(MAIN_LOOP_SECONDS);
2013-12-16 23:25:33 -04:00
2012-08-21 23:19:50 -03:00
// init the optical flow sensor
if(g.optflow_enabled) {
init_optflow();
}
2011-12-23 18:42:05 -04:00
2012-12-04 01:00:51 -04:00
// initialise inertial nav
inertial_nav.init();
2011-07-30 17:42:54 -03:00
2011-10-18 03:51:47 -03:00
#ifdef USERHOOK_INIT
2012-08-21 23:19:50 -03:00
USERHOOK_INIT
2011-10-18 03:51:47 -03:00
#endif
2011-11-19 18:01:47 -04:00
2013-03-18 02:07:04 -03:00
#if CLI_ENABLED == ENABLED
2012-11-21 02:08:03 -04:00
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
2013-11-23 06:45:42 -04:00
if (gcs[1].initialised) {
2013-09-19 03:24:23 -03:00
hal.uartC->println_P(msg);
}
2013-11-23 06:45:42 -04:00
if (num_gcs > 2 && gcs[2].initialised) {
hal.uartD->println_P(msg);
}
2011-07-17 07:34:05 -03:00
#endif // CLI_ENABLED
2013-03-18 02:19:31 -03:00
#if HIL_MODE != HIL_MODE_DISABLED
while (!barometer.healthy) {
// the barometer becomes healthy when we get the first
// HIL_STATE message
gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
delay(1000);
}
#endif
2012-08-21 23:19:50 -03:00
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
2014-01-15 10:18:23 -04:00
init_barometer(true);
2012-08-21 23:19:50 -03:00
#endif
// initialise sonar
#if CONFIG_SONAR == ENABLED
init_sonar();
#endif
// initialize commands
// -------------------
init_commands();
2013-07-16 10:05:59 -03:00
// initialise the flight mode and aux switch
2012-08-21 23:19:50 -03:00
// ---------------------------
reset_control_switch();
2013-07-16 10:05:59 -03:00
init_aux_switches();
2012-08-21 23:19:50 -03:00
2013-11-10 08:05:58 -04:00
#if FRAME_CONFIG == HELI_FRAME
// trad heli specific initialisation
heli_init();
#endif
2013-11-10 08:51:54 -04:00
startup_ground(true);
2011-07-30 17:42:54 -03:00
2011-11-20 04:49:56 -04:00
#if LOGGING_ENABLED == ENABLED
2012-08-21 23:19:50 -03:00
Log_Write_Startup();
2011-11-20 04:49:56 -04:00
#endif
2011-06-16 14:03:26 -03:00
2012-12-13 15:48:01 -04:00
cliSerial->print_P(PSTR("\nReady to FLY "));
}
2012-07-14 23:26:17 -03:00
2012-12-13 15:48:01 -04:00
//******************************************************************************
2011-07-30 17:42:54 -03:00
//This function does all the calibrations, etc. that we need during a ground start
2012-12-13 15:48:01 -04:00
//******************************************************************************
2013-10-29 19:23:47 -03:00
static void startup_ground(bool force_gyro_cal)
2011-07-30 17:42:54 -03:00
{
2012-08-21 23:19:50 -03:00
gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
2011-07-30 17:42:54 -03:00
2013-01-13 01:04:04 -04:00
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
2012-08-21 23:19:50 -03:00
// Warm up and read Gyro offsets
// -----------------------------
2013-10-29 19:23:47 -03:00
ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START,
2013-09-19 05:33:21 -03:00
ins_sample_rate);
2012-08-21 23:19:50 -03:00
#if CLI_ENABLED == ENABLED
2012-11-05 00:32:38 -04:00
report_ins();
2012-08-21 23:19:50 -03:00
#endif
2011-07-30 17:42:54 -03:00
2012-08-21 02:38:31 -03:00
// setup fast AHRS gains to get right attitude
ahrs.set_fast_gains(true);
2013-07-28 04:45:39 -03:00
// set landed flag
set_land_complete(true);
2011-04-16 17:44:44 -03:00
}
2013-07-19 23:01:10 -03:00
// returns true if the GPS is ok and home position is set
static bool GPS_ok()
{
2013-11-11 09:24:18 -04:00
if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D && !gps_glitch.glitching() && !failsafe.gps) {
2013-07-19 23:01:10 -03:00
return true;
}else{
return false;
}
}
2013-05-29 17:12:24 -03:00
// returns true or false whether mode requires GPS
static bool mode_requires_GPS(uint8_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
2013-10-29 01:28:27 -03:00
case LOITER:
2013-05-29 17:12:24 -03:00
case RTL:
case CIRCLE:
case POSITION:
2013-11-13 13:57:48 -04:00
case DRIFT:
2013-05-29 17:12:24 -03:00
return true;
default:
return false;
2013-10-29 01:28:27 -03:00
}
2013-05-29 17:12:24 -03:00
return false;
}
2013-08-04 03:56:35 -03:00
// manual_flight_mode - returns true if flight mode is completely manual (i.e. roll, pitch and yaw controlled by pilot)
static bool manual_flight_mode(uint8_t mode) {
switch(mode) {
case ACRO:
case STABILIZE:
2013-11-13 13:57:48 -04:00
case DRIFT:
2013-08-04 03:56:35 -03:00
case SPORT:
return true;
default:
return false;
}
return false;
}
2012-12-08 01:23:32 -04:00
// set_mode - change flight mode and perform any necessary initialisation
2013-11-06 01:51:28 -04:00
// optional force parameter used to force the flight mode change (used only first time mode is set)
2013-07-19 23:01:10 -03:00
// returns true if mode was succesfully set
2013-11-16 00:56:15 -04:00
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
2013-07-19 23:01:10 -03:00
static bool set_mode(uint8_t mode)
2012-08-21 23:19:50 -03:00
{
2013-07-19 23:01:10 -03:00
// boolean to record if flight mode could be set
bool success = false;
2013-08-22 01:25:27 -03:00
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
2012-08-21 23:19:50 -03:00
2013-10-14 23:53:56 -03:00
// return immediately if we are already in the desired mode
if (mode == control_mode) {
return true;
}
2013-07-19 23:01:10 -03:00
switch(mode) {
case ACRO:
2014-01-11 04:03:48 -04:00
success = acro_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case STABILIZE:
2014-01-11 04:03:48 -04:00
success = stabilize_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case ALT_HOLD:
2014-01-11 04:03:48 -04:00
success = althold_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case AUTO:
2014-01-23 23:28:42 -04:00
success = auto_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case CIRCLE:
2014-01-23 23:28:42 -04:00
success = circle_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case LOITER:
2014-01-23 23:28:42 -04:00
success = loiter_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case POSITION:
2013-08-22 01:25:27 -03:00
if (GPS_ok() || ignore_checks) {
2013-07-19 23:01:10 -03:00
success = true;
}
break;
case GUIDED:
2014-01-23 23:28:42 -04:00
success = guided_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case LAND:
2014-01-11 04:03:48 -04:00
success = land_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case RTL:
2014-01-23 23:28:42 -04:00
success = rtl_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
case OF_LOITER:
2014-01-23 23:28:42 -04:00
success = ofloiter_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
2013-11-13 13:57:48 -04:00
case DRIFT:
2014-01-11 04:03:48 -04:00
success = drift_init(ignore_checks);
2013-07-19 23:01:10 -03:00
break;
2013-08-04 03:56:35 -03:00
case SPORT:
2014-01-11 04:03:48 -04:00
success = sport_init(ignore_checks);
2013-08-04 05:46:04 -03:00
// reset acro angle targets to current attitude
acro_roll = ahrs.roll_sensor;
acro_pitch = ahrs.pitch_sensor;
2013-11-27 09:48:20 -04:00
control_yaw = ahrs.yaw_sensor;
2013-08-04 03:56:35 -03:00
break;
2013-07-19 23:01:10 -03:00
default:
success = false;
break;
2012-08-21 23:19:50 -03:00
}
2011-09-04 21:15:36 -03:00
2013-07-19 23:01:10 -03:00
// update flight mode
if (success) {
control_mode = mode;
Log_Write_Mode(control_mode);
}else{
// Log error that we failed to enter desired flight mode
2013-09-24 08:47:54 -03:00
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
2012-08-21 23:19:50 -03:00
}
2013-07-19 23:01:10 -03:00
// return success or failure
return success;
2011-04-16 17:44:44 -03:00
}
2013-04-17 09:25:32 -03:00
// update_auto_armed - update status of auto_armed flag
static void update_auto_armed()
{
// disarm checks
if(ap.auto_armed){
// if motors are disarmed, auto_armed should also be false
if(!motors.armed()) {
set_auto_armed(false);
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
2013-09-26 05:54:33 -03:00
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio) {
2013-04-17 09:25:32 -03:00
set_auto_armed(false);
}
}else{
// arm checks
2013-07-19 17:16:37 -03:00
#if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
2013-11-04 07:54:47 -04:00
if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete()) {
set_auto_armed(true);
}
2013-07-19 17:16:37 -03:00
#else
2013-04-17 09:25:32 -03:00
// if motors are armed and throttle is above zero auto_armed should be true
if(motors.armed() && g.rc_3.control_in != 0) {
set_auto_armed(true);
}
2013-11-04 07:54:47 -04:00
#endif // HELI_FRAME
2013-04-17 09:25:32 -03:00
}
}
2011-08-01 08:39:17 -03:00
/*
2012-08-21 23:19:50 -03:00
* map from a 8 bit EEPROM baud rate to a real baud rate
2011-08-01 08:39:17 -03:00
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
2012-02-04 04:06:22 -04:00
case 1: return 1200;
case 2: return 2400;
case 4: return 4800;
2011-08-01 08:39:17 -03:00
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
2013-11-25 19:58:50 -04:00
//cliSerial->println_P(PSTR("Invalid baudrate"));
2011-08-01 08:39:17 -03:00
return default_baud;
}
2011-11-20 05:42:51 -04:00
static void check_usb_mux(void)
{
2013-09-19 03:24:23 -03:00
bool usb_check = hal.gpio->usb_connected();
2013-10-08 03:25:14 -03:00
if (usb_check == ap.usb_connected) {
2011-11-20 05:42:51 -04:00
return;
}
// the user has switched to/from the telemetry port
2013-10-08 03:25:14 -03:00
ap.usb_connected = usb_check;
2013-09-20 20:21:17 -03:00
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
// the APM2 has a MUX setup where the first serial port switches
// between USB and a TTL serial connection. When on USB we use
// SERIAL0_BAUD, but when connected as a TTL serial port we run it
2013-11-25 19:58:50 -04:00
// at SERIAL1_BAUD.
2013-10-08 03:25:14 -03:00
if (ap.usb_connected) {
2012-12-13 19:27:42 -04:00
hal.uartA->begin(SERIAL0_BAUD);
2011-11-20 05:42:51 -04:00
} else {
2013-11-25 19:58:50 -04:00
hal.uartA->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD));
2011-11-20 05:42:51 -04:00
}
2013-09-20 20:21:17 -03:00
#endif
2011-11-20 05:42:51 -04:00
}
2011-12-13 03:19:41 -04:00
2012-10-22 04:45:24 -03:00
//
// print_flight_mode - prints flight mode to serial port.
//
static void
2013-04-20 02:18:22 -03:00
print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
2012-10-22 04:45:24 -03:00
{
switch (mode) {
case STABILIZE:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("STABILIZE"));
2012-10-22 04:45:24 -03:00
break;
case ACRO:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("ACRO"));
2012-10-22 04:45:24 -03:00
break;
case ALT_HOLD:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("ALT_HOLD"));
2012-10-22 04:45:24 -03:00
break;
case AUTO:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("AUTO"));
2012-10-22 04:45:24 -03:00
break;
case GUIDED:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("GUIDED"));
2012-10-22 04:45:24 -03:00
break;
case LOITER:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("LOITER"));
2012-10-22 04:45:24 -03:00
break;
case RTL:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("RTL"));
2012-10-22 04:45:24 -03:00
break;
case CIRCLE:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("CIRCLE"));
2012-10-22 04:45:24 -03:00
break;
case POSITION:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("POSITION"));
2012-10-22 04:45:24 -03:00
break;
case LAND:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("LAND"));
2012-10-22 04:45:24 -03:00
break;
case OF_LOITER:
2013-04-20 02:18:22 -03:00
port->print_P(PSTR("OF_LOITER"));
2012-10-22 04:45:24 -03:00
break;
2013-11-13 13:57:48 -04:00
case DRIFT:
port->print_P(PSTR("DRIFT"));
2012-10-22 04:45:24 -03:00
break;
2013-08-04 03:56:35 -03:00
case SPORT:
port->print_P(PSTR("SPORT"));
break;
2012-10-22 04:45:24 -03:00
default:
2013-04-20 02:18:22 -03:00
port->printf_P(PSTR("Mode(%u)"), (unsigned)mode);
2012-10-22 04:45:24 -03:00
break;
}
}