2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
2016-05-05 19:10:08 -03:00
|
|
|
#include "version.h"
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
/*****************************************************************************
|
2012-08-21 23:19:51 -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-09-08 22:29:39 -03:00
|
|
|
*****************************************************************************/
|
|
|
|
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
|
|
|
|
// This is the help function
|
2015-05-13 03:09:36 -03:00
|
|
|
int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("Commands:\n"
|
2012-08-21 23:19:51 -03:00
|
|
|
" logs log readback/setup mode\n"
|
|
|
|
" setup setup mode\n"
|
|
|
|
" test test mode\n"
|
2012-11-24 03:14:26 -04:00
|
|
|
" reboot reboot to flight mode\n"
|
2015-10-24 18:45:41 -03:00
|
|
|
"\n");
|
2012-08-21 23:19:51 -03:00
|
|
|
return(0);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Command/function table for the top-level menu.
|
2015-10-25 14:03:46 -03:00
|
|
|
static const struct Menu::command main_menu_commands[] = {
|
2011-09-08 22:29:39 -03:00
|
|
|
// command function called
|
|
|
|
// ======= ===============
|
2015-05-13 03:09:36 -03:00
|
|
|
{"logs", MENU_FUNC(process_logs)},
|
|
|
|
{"setup", MENU_FUNC(setup_mode)},
|
|
|
|
{"test", MENU_FUNC(test_mode)},
|
|
|
|
{"reboot", MENU_FUNC(reboot_board)},
|
|
|
|
{"help", MENU_FUNC(main_menu_help)},
|
2011-09-08 22:29:39 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
// Create the top-level menu object.
|
2011-09-17 15:25:31 -03:00
|
|
|
MENU(main_menu, THISFIRMWARE, main_menu_commands);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
|
2012-11-24 03:14:26 -04:00
|
|
|
{
|
2013-09-03 22:59:16 -03:00
|
|
|
hal.scheduler->reboot(false);
|
2012-11-24 03:14:26 -04:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2011-10-27 04:35:25 -03:00
|
|
|
// the user wants the CLI. It never exits
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::run_cli(AP_HAL::UARTDriver *port)
|
2011-10-27 04:35:25 -03:00
|
|
|
{
|
2011-12-21 08:25:51 -04:00
|
|
|
// disable the failsafe code in the CLI
|
2016-10-28 19:10:03 -03:00
|
|
|
hal.scheduler->register_timer_failsafe(nullptr,1);
|
2011-12-21 08:25:51 -04:00
|
|
|
|
2012-12-23 17:51:33 -04:00
|
|
|
// disable the mavlink delay callback
|
2016-10-28 19:10:03 -03:00
|
|
|
hal.scheduler->register_delay_callback(nullptr, 5);
|
2012-12-23 17:51:33 -04:00
|
|
|
|
2012-11-21 01:19:16 -04:00
|
|
|
cliSerial = port;
|
|
|
|
Menu::set_port(port);
|
|
|
|
port->set_blocking_writes(true);
|
|
|
|
|
2011-10-27 04:35:25 -03:00
|
|
|
while (1) {
|
|
|
|
main_menu.run();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
#endif // CLI_ENABLED
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
static void mavlink_delay_cb_static()
|
|
|
|
{
|
|
|
|
plane.mavlink_delay_cb();
|
|
|
|
}
|
|
|
|
|
|
|
|
static void failsafe_check_static()
|
|
|
|
{
|
|
|
|
plane.failsafe_check();
|
|
|
|
}
|
|
|
|
|
|
|
|
void Plane::init_ardupilot()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-01-23 09:45:24 -04:00
|
|
|
// initialise serial port
|
|
|
|
serial_manager.init_console();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-10-25 17:10:41 -03:00
|
|
|
cliSerial->printf("\n\nInit " FIRMWARE_STRING
|
2015-10-24 18:45:41 -03:00
|
|
|
"\n\nFree RAM: %u\n",
|
2015-12-29 03:52:00 -04:00
|
|
|
(unsigned)hal.util->available_memory());
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2011-11-13 00:10:30 -04:00
|
|
|
|
|
|
|
//
|
2012-12-04 18:22:21 -04:00
|
|
|
// Check the EEPROM format version before loading any parameters from EEPROM
|
2012-08-21 23:19:51 -03:00
|
|
|
//
|
2012-02-12 04:20:56 -04:00
|
|
|
load_parameters();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2016-10-25 22:24:41 -03:00
|
|
|
// initialise stats module
|
|
|
|
g2.stats.init();
|
|
|
|
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2015-03-13 08:33:48 -03:00
|
|
|
if (g.hil_mode == 1) {
|
|
|
|
// set sensors to HIL mode
|
|
|
|
ins.set_hil_mode();
|
|
|
|
compass.set_hil_mode();
|
|
|
|
barometer.set_hil_mode();
|
|
|
|
}
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2015-03-13 08:33:48 -03:00
|
|
|
|
2016-06-30 03:48:30 -03:00
|
|
|
set_control_channels();
|
|
|
|
|
2016-09-10 19:19:22 -03:00
|
|
|
#if HAVE_PX4_MIXER
|
2016-08-11 00:19:00 -03:00
|
|
|
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);
|
2015-06-02 08:59:03 -03:00
|
|
|
}
|
|
|
|
}
|
2014-11-05 06:18:04 -04:00
|
|
|
#endif
|
|
|
|
|
2016-05-30 22:35:35 -03:00
|
|
|
gcs().set_dataflash(&DataFlash);
|
2014-01-19 21:58:49 -04:00
|
|
|
|
2016-10-16 19:20:55 -03:00
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
|
2015-01-23 09:45:24 -04:00
|
|
|
// initialise serial ports
|
|
|
|
serial_manager.init();
|
2016-08-25 04:48:27 -03:00
|
|
|
gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
|
2015-01-23 09:45:24 -04:00
|
|
|
|
2017-02-14 08:07:54 -04:00
|
|
|
|
|
|
|
// specify callback function for CLI menu system
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
if (g.cli_enabled) {
|
|
|
|
gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Plane::run_cli, void, AP_HAL::UARTDriver *));
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2016-08-03 04:17:38 -03:00
|
|
|
// 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();
|
2016-02-21 20:06:09 -04:00
|
|
|
|
2017-03-01 03:24:32 -04:00
|
|
|
relay.init();
|
|
|
|
|
2017-01-21 02:16:27 -04:00
|
|
|
// initialise notify system
|
|
|
|
notify.init(false);
|
|
|
|
notify_flight_mode(control_mode);
|
|
|
|
|
2016-11-12 23:55:35 -04:00
|
|
|
init_rc_out_main();
|
|
|
|
|
2014-01-20 00:36:31 -04:00
|
|
|
// allow servo set on all channels except first 4
|
|
|
|
ServoRelayEvents.set_channel_mask(0xFFF0);
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
// 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);
|
|
|
|
|
2013-05-05 21:57:57 -03:00
|
|
|
// init baro before we start the GCS, so that the CLI baro test works
|
|
|
|
barometer.init();
|
|
|
|
|
2014-08-26 08:17:47 -03:00
|
|
|
// initialise rangefinder
|
|
|
|
init_rangefinder();
|
2013-10-30 19:23:21 -03:00
|
|
|
|
2014-08-08 23:29:20 -03:00
|
|
|
// initialise battery monitoring
|
|
|
|
battery.init();
|
|
|
|
|
2015-09-24 07:58:18 -03:00
|
|
|
rpm_sensor.init();
|
|
|
|
|
2013-09-20 20:16:17 -03:00
|
|
|
// 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();
|
2013-09-19 03:23:58 -03:00
|
|
|
|
2016-05-16 00:33:43 -03:00
|
|
|
// setup telem slots with serial ports
|
2016-08-25 04:48:27 -03:00
|
|
|
gcs().setup_uarts(serial_manager);
|
2015-05-15 01:24:39 -03:00
|
|
|
|
2015-01-23 09:45:24 -04:00
|
|
|
// setup frsky
|
|
|
|
#if FRSKY_TELEM_ENABLED == ENABLED
|
2016-05-03 12:05:09 -03:00
|
|
|
// setup frsky, and pass a number of parameters to the library
|
2016-09-01 15:11:17 -03:00
|
|
|
frsky_telemetry.init(serial_manager, FIRMWARE_STRING,
|
|
|
|
MAV_TYPE_FIXED_WING,
|
2016-09-25 13:19:49 -03:00
|
|
|
&g.fs_batt_voltage, &g.fs_batt_mah);
|
2013-11-25 19:58:11 -04:00
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2011-12-21 08:29:22 -04:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2015-05-13 03:09:36 -03:00
|
|
|
log_init();
|
2011-12-21 08:29:22 -04:00
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2013-06-02 22:40:44 -03:00
|
|
|
// initialise airspeed sensor
|
|
|
|
airspeed.init();
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
if (g.compass_enabled==true) {
|
2015-08-23 09:21:51 -03:00
|
|
|
bool compass_ok = compass.init() && compass.read();
|
|
|
|
#if HIL_SUPPORT
|
2016-05-16 15:14:56 -03:00
|
|
|
if (g.hil_mode != 0) {
|
2015-08-23 09:21:51 -03:00
|
|
|
compass_ok = true;
|
2015-12-08 08:07:46 -04:00
|
|
|
}
|
2015-08-23 09:21:51 -03:00
|
|
|
#endif
|
|
|
|
if (!compass_ok) {
|
2017-01-21 00:35:24 -04:00
|
|
|
cliSerial->printf("Compass initialisation failed!\n");
|
2011-09-08 22:29:39 -03:00
|
|
|
g.compass_enabled = false;
|
|
|
|
} else {
|
2012-03-11 05:13:31 -03:00
|
|
|
ahrs.set_compass(&compass);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2015-01-02 20:09:02 -04:00
|
|
|
|
2015-07-10 01:50:35 -03:00
|
|
|
#if OPTFLOW == ENABLED
|
2015-01-02 20:09:02 -04:00
|
|
|
// make optflow available to libraries
|
2016-07-11 19:44:04 -03:00
|
|
|
if (optflow.enabled()) {
|
|
|
|
ahrs.set_optflow(&optflow);
|
|
|
|
}
|
2015-07-10 01:50:35 -03:00
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-10 19:57:44 -03:00
|
|
|
// give AHRS the airspeed sensor
|
|
|
|
ahrs.set_airspeed(&airspeed);
|
|
|
|
|
2012-06-10 03:36:18 -03:00
|
|
|
// GPS Initialization
|
2015-01-23 09:45:24 -04:00
|
|
|
gps.init(&DataFlash, serial_manager);
|
2013-12-21 07:27:15 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
init_rc_in(); // sets up rc channels from radio
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-01-27 09:12:12 -04:00
|
|
|
#if MOUNT == ENABLED
|
2015-01-08 16:12:08 -04:00
|
|
|
// initialise camera mount
|
2016-01-05 01:23:47 -04:00
|
|
|
camera_mount.init(&DataFlash, serial_manager);
|
2015-01-27 09:12:12 -04:00
|
|
|
#endif
|
2015-01-08 16:12:08 -04:00
|
|
|
|
2011-12-16 15:28:25 -04:00
|
|
|
#if FENCE_TRIGGERED_PIN > 0
|
2014-06-01 20:28:27 -03:00
|
|
|
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
|
|
|
|
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
|
2011-12-16 15:28:25 -04:00
|
|
|
#endif
|
|
|
|
|
2011-12-22 20:11:59 -04:00
|
|
|
/*
|
2012-08-21 23:19:51 -03:00
|
|
|
* setup the 'main loop is dead' check. Note that this relies on
|
|
|
|
* the RC library being initialised.
|
2011-12-22 20:11:59 -04:00
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
|
2011-12-22 20:11:59 -04:00
|
|
|
|
2015-01-23 09:45:24 -04:00
|
|
|
#if CLI_ENABLED == ENABLED
|
2016-08-25 04:48:27 -03:00
|
|
|
gcs().handle_interactive_setup();
|
2015-01-23 09:45:24 -04:00
|
|
|
#endif // CLI_ENABLED
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-07-31 19:39:36 -03:00
|
|
|
init_capabilities();
|
|
|
|
|
2015-11-24 04:24:04 -04:00
|
|
|
quadplane.setup();
|
2016-01-04 18:38:02 -04:00
|
|
|
|
2017-01-09 05:52:49 -04:00
|
|
|
AP_Param::reload_defaults_file();
|
|
|
|
|
2016-04-02 17:49:38 -03:00
|
|
|
startup_ground();
|
|
|
|
|
2016-06-30 03:48:30 -03:00
|
|
|
// don't initialise aux rc output until after quadplane is setup as
|
2016-01-04 18:38:02 -04:00
|
|
|
// that can change initial values of channels
|
2016-06-30 03:48:30 -03:00
|
|
|
init_rc_out_aux();
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2013-04-11 21:25:46 -03:00
|
|
|
// choose the nav controller
|
|
|
|
set_nav_controller();
|
|
|
|
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// set the correct flight mode
|
|
|
|
// ---------------------------
|
|
|
|
reset_control_switch();
|
2014-10-13 04:51:10 -03:00
|
|
|
|
|
|
|
// initialise sensor
|
2014-11-03 03:33:56 -04:00
|
|
|
#if OPTFLOW == ENABLED
|
2016-07-11 19:44:04 -03:00
|
|
|
if (optflow.enabled()) {
|
|
|
|
optflow.init();
|
2016-07-11 17:46:16 -03:00
|
|
|
}
|
2014-11-03 03:33:56 -04:00
|
|
|
#endif
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
//********************************************************************************
|
|
|
|
//This function does all the calibrations, etc. that we need during a ground start
|
|
|
|
//********************************************************************************
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::startup_ground(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(INITIALISING, MODE_REASON_UNKNOWN);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
#if (GROUND_START_DELAY > 0)
|
2017-03-04 04:31:24 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_NOTICE,"Ground start with delay");
|
2012-08-21 23:19:51 -03:00
|
|
|
delay(GROUND_START_DELAY * 1000);
|
2017-03-04 04:31:24 -04:00
|
|
|
#else
|
|
|
|
gcs_send_text(MAV_SEVERITY_INFO,"Ground start");
|
2012-08-21 23:19:51 -03:00
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-11-05 00:32:13 -04:00
|
|
|
//INS ground start
|
2012-08-21 23:19:51 -03:00
|
|
|
//------------------------
|
2011-09-08 22:29:39 -03:00
|
|
|
//
|
2015-03-10 20:16:59 -03:00
|
|
|
startup_INS_ground();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// read the radio to set trims
|
|
|
|
// ---------------------------
|
2015-03-06 17:20:55 -04:00
|
|
|
if (g.trim_rc_at_start != 0) {
|
|
|
|
trim_radio();
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// Save the settings for in-air restart
|
|
|
|
// ------------------------------------
|
|
|
|
//save_EEPROM_groundstart();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2014-03-11 01:06:45 -03:00
|
|
|
// initialise mission library
|
|
|
|
mission.init();
|
|
|
|
|
2012-12-25 17:46:36 -04:00
|
|
|
// reset last heartbeat time, so we don't trigger failsafe on slow
|
|
|
|
// startup
|
2015-05-13 19:05:32 -03:00
|
|
|
failsafe.last_heartbeat_ms = millis();
|
2012-12-25 17:46:36 -04:00
|
|
|
|
2012-03-30 03:06:03 -03:00
|
|
|
// 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
|
2015-01-23 09:45:24 -04:00
|
|
|
serial_manager.set_blocking_writes_all(false);
|
2012-03-30 03:06:03 -03:00
|
|
|
|
2015-05-06 23:11:52 -03:00
|
|
|
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
2016-02-21 20:06:09 -04:00
|
|
|
ins.set_dataflash(&DataFlash);
|
|
|
|
|
2017-03-04 04:31:24 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO,"Ground start complete");
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
enum FlightMode Plane::get_previous_mode() {
|
2014-02-13 18:49:42 -04:00
|
|
|
return previous_mode;
|
|
|
|
}
|
|
|
|
|
2016-08-13 04:54:37 -03:00
|
|
|
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
if(control_mode == mode) {
|
|
|
|
// don't switch modes if we are already in the correct mode.
|
|
|
|
return;
|
|
|
|
}
|
2017-01-20 06:15:51 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
if(g.auto_trim > 0 && control_mode == MANUAL)
|
|
|
|
trim_control_surfaces();
|
|
|
|
|
2014-03-14 09:40:47 -03:00
|
|
|
// perform any cleanup required for prev flight mode
|
|
|
|
exit_mode(control_mode);
|
|
|
|
|
2014-06-05 03:12:10 -03:00
|
|
|
// cancel inverted flight
|
|
|
|
auto_state.inverted_flight = false;
|
|
|
|
|
2014-08-04 07:39:15 -03:00
|
|
|
// don't cross-track when starting a mission
|
|
|
|
auto_state.next_wp_no_crosstrack = true;
|
|
|
|
|
2014-10-17 22:46:20 -03:00
|
|
|
// reset landing check
|
2016-11-23 07:07:50 -04:00
|
|
|
auto_state.checked_for_autoland = false;
|
2016-02-08 23:17:10 -04:00
|
|
|
|
2014-10-06 17:17:33 -03:00
|
|
|
// zero locked course
|
|
|
|
steer_state.locked_course_err = 0;
|
|
|
|
|
2015-06-03 16:22:24 -03:00
|
|
|
// reset crash detection
|
2015-08-20 17:44:32 -03:00
|
|
|
crash_state.is_crashed = false;
|
2015-11-17 20:11:21 -04:00
|
|
|
crash_state.impact_detected = false;
|
2015-06-03 16:22:24 -03:00
|
|
|
|
2016-05-09 11:34:11 -03:00
|
|
|
// reset external attitude guidance
|
2016-06-30 12:50:17 -03:00
|
|
|
guided_state.last_forced_rpy_ms.zero();
|
|
|
|
guided_state.last_forced_throttle_ms = 0;
|
2016-05-09 11:34:11 -03:00
|
|
|
|
2014-03-14 09:40:47 -03:00
|
|
|
// set mode
|
2014-02-13 18:49:42 -04:00
|
|
|
previous_mode = control_mode;
|
2012-08-21 23:19:51 -03:00
|
|
|
control_mode = mode;
|
2016-08-13 04:54:37 -03:00
|
|
|
previous_mode_reason = control_mode_reason;
|
|
|
|
control_mode_reason = reason;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2016-08-09 19:27:51 -03:00
|
|
|
#if FRSKY_TELEM_ENABLED == ENABLED
|
|
|
|
frsky_telemetry.update_control_mode(control_mode);
|
|
|
|
#endif
|
|
|
|
|
2014-04-12 01:12:14 -03:00
|
|
|
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
|
|
|
|
// restore last gains
|
|
|
|
autotune_restore();
|
|
|
|
}
|
|
|
|
|
2014-08-23 04:34:07 -03:00
|
|
|
// zero initial pitch and highest airspeed on mode change
|
|
|
|
auto_state.highest_airspeed = 0;
|
|
|
|
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
|
|
|
|
|
|
|
|
// disable taildrag takeoff on mode change
|
|
|
|
auto_state.fbwa_tdrag_takeoff_mode = false;
|
|
|
|
|
2015-08-12 01:42:28 -03:00
|
|
|
// start with previous WP at current location
|
|
|
|
prev_WP_loc = current_loc;
|
|
|
|
|
2015-09-16 06:03:28 -03:00
|
|
|
// new mode means new loiter
|
|
|
|
loiter.start_time_ms = 0;
|
|
|
|
|
2016-01-03 06:38:51 -04:00
|
|
|
// assume non-VTOL mode
|
|
|
|
auto_state.vtol_mode = false;
|
2016-05-11 02:57:41 -03:00
|
|
|
auto_state.vtol_loiter = false;
|
2016-01-03 06:38:51 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
switch(control_mode)
|
|
|
|
{
|
|
|
|
case INITIALISING:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2014-04-12 01:12:14 -03:00
|
|
|
break;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
case MANUAL:
|
|
|
|
case STABILIZE:
|
2012-12-04 02:32:37 -04:00
|
|
|
case TRAINING:
|
2012-08-21 23:19:51 -03:00
|
|
|
case FLY_BY_WIRE_A:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = false;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2014-04-12 01:12:14 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AUTOTUNE:
|
|
|
|
auto_throttle_mode = false;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2014-04-12 01:12:14 -03:00
|
|
|
autotune_start();
|
2013-03-27 20:27:25 -03:00
|
|
|
break;
|
|
|
|
|
2013-07-12 08:34:32 -03:00
|
|
|
case ACRO:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = false;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2013-07-12 08:34:32 -03:00
|
|
|
acro_state.locked_roll = false;
|
|
|
|
acro_state.locked_pitch = false;
|
|
|
|
break;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2013-07-13 07:05:53 -03:00
|
|
|
case CRUISE:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2013-07-13 07:05:53 -03:00
|
|
|
cruise_state.locked_heading = false;
|
|
|
|
cruise_state.lock_timer_ms = 0;
|
2017-02-26 19:18:38 -04:00
|
|
|
|
|
|
|
// for ArduSoar soaring_controller
|
|
|
|
g2.soaring_controller.init_cruising();
|
|
|
|
|
2014-07-24 03:21:30 -03:00
|
|
|
set_target_altitude_current();
|
2013-07-13 07:05:53 -03:00
|
|
|
break;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
case FLY_BY_WIRE_B:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2017-02-26 19:18:38 -04:00
|
|
|
|
|
|
|
// for ArduSoar soaring_controller
|
|
|
|
g2.soaring_controller.init_cruising();
|
|
|
|
|
2014-07-24 03:21:30 -03:00
|
|
|
set_target_altitude_current();
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
|
2013-02-10 22:52:25 -04:00
|
|
|
case CIRCLE:
|
|
|
|
// the altitude to circle at is taken from the current altitude
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = true;
|
2014-03-16 01:53:10 -03:00
|
|
|
next_WP_loc.alt = current_loc.alt;
|
2013-02-10 22:52:25 -04:00
|
|
|
break;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
case AUTO:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = true;
|
2016-04-22 07:20:45 -03:00
|
|
|
if (quadplane.available() && quadplane.enable == 2) {
|
|
|
|
auto_state.vtol_mode = true;
|
|
|
|
} else {
|
|
|
|
auto_state.vtol_mode = false;
|
|
|
|
}
|
2014-05-18 03:14:11 -03:00
|
|
|
next_WP_loc = prev_WP_loc = current_loc;
|
2014-04-28 18:50:51 -03:00
|
|
|
// start or resume the mission, based on MIS_AUTORESET
|
|
|
|
mission.start_or_resume();
|
2017-02-26 19:18:38 -04:00
|
|
|
|
|
|
|
g2.soaring_controller.init_cruising();
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case RTL:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = true;
|
2014-03-16 01:53:10 -03:00
|
|
|
prev_WP_loc = current_loc;
|
2016-04-29 02:31:08 -03:00
|
|
|
do_RTL(get_RTL_altitude());
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case LOITER:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = true;
|
2012-08-21 23:19:51 -03:00
|
|
|
do_loiter_at_location();
|
2017-02-26 19:18:38 -04:00
|
|
|
|
|
|
|
if (g2.soaring_controller.is_active() &&
|
|
|
|
g2.soaring_controller.suppress_throttle()) {
|
|
|
|
g2.soaring_controller.init_thermalling();
|
|
|
|
g2.soaring_controller.get_target(next_WP_loc); // ahead on flight path
|
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2012-08-21 23:19:51 -03:00
|
|
|
case GUIDED:
|
2014-04-12 01:12:14 -03:00
|
|
|
auto_throttle_mode = true;
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = true;
|
2013-09-07 18:31:10 -03:00
|
|
|
guided_throttle_passthru = false;
|
2015-05-10 01:28:02 -03:00
|
|
|
/*
|
|
|
|
when entering guided mode we set the target as the current
|
|
|
|
location. This matches the behaviour of the copter code
|
|
|
|
*/
|
|
|
|
guided_WP_loc = current_loc;
|
2012-08-21 23:19:51 -03:00
|
|
|
set_guided_WP();
|
|
|
|
break;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2015-12-26 03:45:42 -04:00
|
|
|
case QSTABILIZE:
|
|
|
|
case QHOVER:
|
2015-12-26 04:51:05 -04:00
|
|
|
case QLOITER:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLAND:
|
2016-04-29 02:31:08 -03:00
|
|
|
case QRTL:
|
2016-07-04 23:52:13 -03:00
|
|
|
auto_navigation_mode = false;
|
2015-12-26 06:40:40 -04:00
|
|
|
if (!quadplane.init_mode()) {
|
|
|
|
control_mode = previous_mode;
|
|
|
|
} else {
|
|
|
|
auto_throttle_mode = false;
|
2016-01-03 06:38:51 -04:00
|
|
|
auto_state.vtol_mode = true;
|
2015-12-26 06:40:40 -04:00
|
|
|
}
|
2015-12-26 04:51:05 -04:00
|
|
|
break;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
2014-04-12 01:12:14 -03:00
|
|
|
// start with throttle suppressed in auto_throttle modes
|
|
|
|
throttle_suppressed = auto_throttle_mode;
|
2012-08-27 03:26:53 -03:00
|
|
|
|
2016-07-10 21:23:08 -03:00
|
|
|
adsb.set_is_auto_mode(auto_navigation_mode);
|
|
|
|
|
2014-01-13 22:07:43 -04:00
|
|
|
if (should_log(MASK_LOG_MODE))
|
2015-01-16 12:30:49 -04:00
|
|
|
DataFlash.Log_Write_Mode(control_mode);
|
2013-06-01 09:29:28 -03:00
|
|
|
|
2017-01-20 06:15:51 -04:00
|
|
|
// update notify with flight mode change
|
|
|
|
notify_flight_mode(control_mode);
|
|
|
|
|
2016-12-14 05:22:56 -04:00
|
|
|
// reset steering integrator on mode change
|
2014-08-24 19:20:37 -03:00
|
|
|
steerController.reset_I();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2014-10-01 01:19:20 -03:00
|
|
|
/*
|
|
|
|
set_mode() wrapper for MAVLink SET_MODE
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::mavlink_set_mode(uint8_t mode)
|
2014-10-01 01:19:20 -03:00
|
|
|
{
|
|
|
|
switch (mode) {
|
|
|
|
case MANUAL:
|
|
|
|
case CIRCLE:
|
|
|
|
case STABILIZE:
|
|
|
|
case TRAINING:
|
|
|
|
case ACRO:
|
|
|
|
case FLY_BY_WIRE_A:
|
|
|
|
case AUTOTUNE:
|
|
|
|
case FLY_BY_WIRE_B:
|
|
|
|
case CRUISE:
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2015-05-10 01:28:02 -03:00
|
|
|
case GUIDED:
|
2014-10-01 01:19:20 -03:00
|
|
|
case AUTO:
|
|
|
|
case RTL:
|
|
|
|
case LOITER:
|
2015-12-26 03:45:42 -04:00
|
|
|
case QSTABILIZE:
|
|
|
|
case QHOVER:
|
2015-12-26 04:51:05 -04:00
|
|
|
case QLOITER:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLAND:
|
2016-04-29 02:31:08 -03:00
|
|
|
case QRTL:
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
|
2014-10-01 01:19:20 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2014-03-14 09:40:47 -03:00
|
|
|
// exit_mode - perform any cleanup required when leaving a flight mode
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::exit_mode(enum FlightMode mode)
|
2014-03-14 09:40:47 -03:00
|
|
|
{
|
|
|
|
// stop mission when we leave auto
|
|
|
|
if (mode == AUTO) {
|
|
|
|
if (mission.state() == AP_Mission::MISSION_RUNNING) {
|
|
|
|
mission.stop();
|
2015-05-09 13:36:40 -03:00
|
|
|
|
|
|
|
if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND)
|
|
|
|
{
|
2016-11-16 21:19:58 -04:00
|
|
|
landing.restart_landing_sequence();
|
2015-05-09 13:36:40 -03:00
|
|
|
}
|
2014-03-14 09:40:47 -03:00
|
|
|
}
|
2015-06-03 16:22:24 -03:00
|
|
|
auto_state.started_flying_in_auto_ms = 0;
|
2014-03-14 09:40:47 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::check_long_failsafe()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-05-13 19:05:32 -03:00
|
|
|
uint32_t tnow = millis();
|
2012-08-21 23:19:51 -03:00
|
|
|
// only act on changes
|
|
|
|
// -------------------
|
2017-01-11 01:29:03 -04:00
|
|
|
if (failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
ArduPlane failsafes: remove rc_override_active
- rc_override_active is never set anywhere in the ArduPlane code; it's only used for Copter and Rover. Removing it significantly simplifies the failsafe code.
- modified code has been tested in SITL. Heartbeat and RC failures in AUTO, CRUISE, and RTL modes (covering the three cases in the failsafe check functions) have been simulated with FS_LONG_ACTN = 0, 1, and 2, FS_SHORT_ACTN = 0, 1, and 2, and FS_GCS_ENABL = 0, 1, and 2. In all cases the results are identical to those with the original code.
2014-09-16 00:14:40 -03:00
|
|
|
if (failsafe.state == FAILSAFE_SHORT &&
|
2013-12-19 20:39:00 -04:00
|
|
|
(tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
|
2016-08-13 04:54:37 -03:00
|
|
|
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
|
2015-05-09 07:51:24 -03:00
|
|
|
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO &&
|
|
|
|
failsafe.last_heartbeat_ms != 0 &&
|
|
|
|
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
|
2016-08-13 04:54:37 -03:00
|
|
|
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
|
2015-05-09 07:51:24 -03:00
|
|
|
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT &&
|
2013-12-19 20:39:00 -04:00
|
|
|
failsafe.last_heartbeat_ms != 0 &&
|
|
|
|
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
|
2016-08-13 04:54:37 -03:00
|
|
|
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
|
2013-12-19 20:39:00 -04:00
|
|
|
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
|
2016-08-25 04:48:27 -03:00
|
|
|
gcs().chan(0).last_radio_status_remrssi_ms != 0 &&
|
|
|
|
(tnow - gcs().chan(0).last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
|
2016-08-13 04:54:37 -03:00
|
|
|
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// We do not change state but allow for user to change mode
|
2013-07-19 01:11:16 -03:00
|
|
|
if (failsafe.state == FAILSAFE_GCS &&
|
|
|
|
(tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
|
|
|
|
failsafe.state = FAILSAFE_NONE;
|
|
|
|
} else if (failsafe.state == FAILSAFE_LONG &&
|
|
|
|
!failsafe.ch3_failsafe) {
|
|
|
|
failsafe.state = FAILSAFE_NONE;
|
2013-07-11 22:56:04 -03:00
|
|
|
}
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::check_short_failsafe()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-21 23:19:51 -03:00
|
|
|
// only act on changes
|
|
|
|
// -------------------
|
2017-01-11 01:29:03 -04:00
|
|
|
if(failsafe.state == FAILSAFE_NONE && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
2016-07-22 10:57:30 -03:00
|
|
|
// The condition is checked and the flag ch3_failsafe is set in radio.cpp
|
|
|
|
if(failsafe.ch3_failsafe) {
|
2016-08-13 04:54:37 -03:00
|
|
|
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-07-19 01:11:16 -03:00
|
|
|
if(failsafe.state == FAILSAFE_SHORT) {
|
|
|
|
if(!failsafe.ch3_failsafe) {
|
2016-08-13 04:54:37 -03:00
|
|
|
failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE);
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::startup_INS_ground(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2015-03-13 08:33:48 -03:00
|
|
|
if (g.hil_mode == 1) {
|
|
|
|
while (barometer.get_last_update() == 0) {
|
|
|
|
// the barometer begins updating when we get the first
|
|
|
|
// HIL_STATE message
|
2015-10-24 19:36:35 -03:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
|
2015-03-13 08:33:48 -03:00
|
|
|
hal.scheduler->delay(1000);
|
|
|
|
}
|
2013-01-03 20:29:19 -04:00
|
|
|
}
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2013-01-03 20:29:19 -04:00
|
|
|
|
2015-09-17 09:32:06 -03:00
|
|
|
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane");
|
2015-05-13 03:09:36 -03:00
|
|
|
hal.scheduler->delay(100);
|
2013-07-19 22:33:09 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2013-01-13 01:04:18 -04:00
|
|
|
ahrs.init();
|
|
|
|
ahrs.set_fly_forward(true);
|
2014-04-21 04:12:24 -03:00
|
|
|
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
|
2013-05-23 22:21:32 -03:00
|
|
|
ahrs.set_wind_estimation(true);
|
2013-01-13 01:04:18 -04:00
|
|
|
|
2015-12-25 23:12:39 -04:00
|
|
|
ins.init(scheduler.get_loop_rate_hz());
|
2012-03-11 05:13:31 -03:00
|
|
|
ahrs.reset();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// read Baro pressure at ground
|
|
|
|
//-----------------------------
|
2016-05-23 22:31:53 -03:00
|
|
|
init_barometer(true);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-07-15 22:21:50 -03:00
|
|
|
if (airspeed.enabled()) {
|
2011-11-20 21:05:43 -04:00
|
|
|
// initialize airspeed sensor
|
|
|
|
// --------------------------
|
2014-11-13 06:12:59 -04:00
|
|
|
zero_airspeed(true);
|
2011-11-20 21:05:43 -04:00
|
|
|
} else {
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING,"No airspeed");
|
2011-11-20 21:05:43 -04:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2013-08-29 00:13:58 -03:00
|
|
|
// updates the status of the notify objects
|
2013-08-24 05:58:32 -03:00
|
|
|
// should be called at 50hz
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::update_notify()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-08-29 00:13:58 -03:00
|
|
|
notify.update();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::resetPerfData(void)
|
2014-10-22 02:14:51 -03:00
|
|
|
{
|
2016-04-20 22:50:13 -03:00
|
|
|
perf.mainLoop_count = 0;
|
|
|
|
perf.G_Dt_max = 0;
|
|
|
|
perf.G_Dt_min = 0;
|
2016-04-21 01:51:29 -03:00
|
|
|
perf.num_long = 0;
|
|
|
|
perf.start_ms = millis();
|
2016-04-21 03:11:48 -03:00
|
|
|
perf.last_log_dropped = DataFlash.num_dropped();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::check_usb_mux(void)
|
2011-11-20 05:31:45 -04:00
|
|
|
{
|
2013-09-19 03:23:58 -03:00
|
|
|
bool usb_check = hal.gpio->usb_connected();
|
2011-11-20 05:31:45 -04:00
|
|
|
if (usb_check == usb_connected) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// the user has switched to/from the telemetry port
|
|
|
|
usb_connected = usb_check;
|
2012-12-20 07:46:48 -04:00
|
|
|
}
|
2011-12-13 03:19:41 -04:00
|
|
|
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
2012-09-18 00:38:56 -03:00
|
|
|
{
|
|
|
|
switch (mode) {
|
|
|
|
case MANUAL:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("Manual");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
|
|
|
case CIRCLE:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("Circle");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
|
|
|
case STABILIZE:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("Stabilize");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
2012-12-04 02:32:37 -04:00
|
|
|
case TRAINING:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("Training");
|
2012-12-04 02:32:37 -04:00
|
|
|
break;
|
2013-07-10 10:25:38 -03:00
|
|
|
case ACRO:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("ACRO");
|
2013-07-10 10:25:38 -03:00
|
|
|
break;
|
2012-09-18 00:38:56 -03:00
|
|
|
case FLY_BY_WIRE_A:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("FBW_A");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
2014-04-12 01:12:14 -03:00
|
|
|
case AUTOTUNE:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("AUTOTUNE");
|
2014-04-12 01:12:14 -03:00
|
|
|
break;
|
2012-09-18 00:38:56 -03:00
|
|
|
case FLY_BY_WIRE_B:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("FBW_B");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
2013-07-13 07:05:53 -03:00
|
|
|
case CRUISE:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("CRUISE");
|
2013-07-13 07:05:53 -03:00
|
|
|
break;
|
2012-09-18 00:38:56 -03:00
|
|
|
case AUTO:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("AUTO");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
|
|
|
case RTL:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("RTL");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
|
|
|
case LOITER:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("Loiter");
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("AVOID_ADSB");
|
2016-08-12 17:27:48 -03:00
|
|
|
break;
|
2014-02-01 23:04:36 -04:00
|
|
|
case GUIDED:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("Guided");
|
2014-02-01 23:04:36 -04:00
|
|
|
break;
|
2016-03-09 02:29:13 -04:00
|
|
|
case QSTABILIZE:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("QStabilize");
|
2016-03-09 02:29:13 -04:00
|
|
|
break;
|
|
|
|
case QHOVER:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("QHover");
|
2016-03-09 02:29:13 -04:00
|
|
|
break;
|
|
|
|
case QLOITER:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("QLoiter");
|
2016-03-09 02:29:13 -04:00
|
|
|
break;
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLAND:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("QLand");
|
2016-03-09 03:20:41 -04:00
|
|
|
break;
|
2016-04-29 02:31:08 -03:00
|
|
|
case QRTL:
|
2017-01-21 00:35:24 -04:00
|
|
|
port->printf("QRTL");
|
2016-04-29 02:31:08 -03:00
|
|
|
break;
|
2012-09-18 00:38:56 -03:00
|
|
|
default:
|
2015-10-25 17:10:41 -03:00
|
|
|
port->printf("Mode(%u)", (unsigned)mode);
|
2012-09-18 00:38:56 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2012-09-18 00:48:08 -03:00
|
|
|
|
2017-01-20 06:15:51 -04:00
|
|
|
// sets notify object flight mode information
|
|
|
|
void Plane::notify_flight_mode(enum FlightMode mode)
|
|
|
|
{
|
|
|
|
AP_Notify::flags.flight_mode = mode;
|
|
|
|
|
|
|
|
// set flight mode string
|
|
|
|
switch (mode) {
|
|
|
|
case MANUAL:
|
|
|
|
notify.set_flight_mode_str("MANU");
|
|
|
|
break;
|
|
|
|
case CIRCLE:
|
|
|
|
notify.set_flight_mode_str("CIRC");
|
|
|
|
break;
|
|
|
|
case STABILIZE:
|
|
|
|
notify.set_flight_mode_str("STAB");
|
|
|
|
break;
|
|
|
|
case TRAINING:
|
|
|
|
notify.set_flight_mode_str("TRAN");
|
|
|
|
break;
|
|
|
|
case ACRO:
|
|
|
|
notify.set_flight_mode_str("ACRO");
|
|
|
|
break;
|
|
|
|
case FLY_BY_WIRE_A:
|
|
|
|
notify.set_flight_mode_str("FBWA");
|
|
|
|
break;
|
|
|
|
case FLY_BY_WIRE_B:
|
|
|
|
notify.set_flight_mode_str("FBWB");
|
|
|
|
break;
|
|
|
|
case CRUISE:
|
|
|
|
notify.set_flight_mode_str("CRUS");
|
|
|
|
break;
|
|
|
|
case AUTOTUNE:
|
|
|
|
notify.set_flight_mode_str("ATUN");
|
|
|
|
break;
|
|
|
|
case AUTO:
|
|
|
|
notify.set_flight_mode_str("AUTO");
|
|
|
|
break;
|
|
|
|
case RTL:
|
|
|
|
notify.set_flight_mode_str("RTL ");
|
|
|
|
break;
|
|
|
|
case LOITER:
|
|
|
|
notify.set_flight_mode_str("LOITER");
|
|
|
|
break;
|
|
|
|
case AVOID_ADSB:
|
|
|
|
notify.set_flight_mode_str("AVOI");
|
|
|
|
break;
|
|
|
|
case GUIDED:
|
|
|
|
notify.set_flight_mode_str("GUID");
|
|
|
|
break;
|
|
|
|
case INITIALISING:
|
|
|
|
notify.set_flight_mode_str("INIT");
|
|
|
|
break;
|
|
|
|
case QSTABILIZE:
|
|
|
|
notify.set_flight_mode_str("QSTB");
|
|
|
|
break;
|
|
|
|
case QHOVER:
|
|
|
|
notify.set_flight_mode_str("QHOV");
|
|
|
|
break;
|
|
|
|
case QLOITER:
|
|
|
|
notify.set_flight_mode_str("QLOT");
|
|
|
|
break;
|
|
|
|
case QLAND:
|
|
|
|
notify.set_flight_mode_str("QLND");
|
|
|
|
break;
|
|
|
|
case QRTL:
|
|
|
|
notify.set_flight_mode_str("QRTL");
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
notify.set_flight_mode_str("----");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-04-24 02:08:06 -03:00
|
|
|
#if CLI_ENABLED == ENABLED
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::print_comma(void)
|
2012-09-18 00:48:08 -03:00
|
|
|
{
|
2017-01-21 00:35:24 -04:00
|
|
|
cliSerial->printf(",");
|
2012-09-18 00:48:08 -03:00
|
|
|
}
|
2015-04-24 02:08:06 -03:00
|
|
|
#endif
|
2012-12-04 18:22:21 -04:00
|
|
|
|
2014-01-13 22:07:43 -04:00
|
|
|
/*
|
|
|
|
should we log a message type now?
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::should_log(uint32_t mask)
|
2014-01-13 22:07:43 -04:00
|
|
|
{
|
2015-08-06 10:25:22 -03:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
2014-01-13 23:29:14 -04:00
|
|
|
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
|
2014-01-13 22:07:43 -04:00
|
|
|
return false;
|
|
|
|
}
|
2016-05-08 23:05:25 -03:00
|
|
|
bool ret = hal.util->get_soft_armed() || DataFlash.log_while_disarmed();
|
2014-01-13 22:51:49 -04:00
|
|
|
if (ret && !DataFlash.logging_started() && !in_log_download) {
|
|
|
|
start_logging();
|
|
|
|
}
|
|
|
|
return ret;
|
2015-08-06 10:25:22 -03:00
|
|
|
#else
|
|
|
|
return false;
|
|
|
|
#endif
|
2014-01-13 22:07:43 -04:00
|
|
|
}
|
2014-07-29 08:38:15 -03:00
|
|
|
|
2014-08-03 04:16:51 -03:00
|
|
|
/*
|
2016-01-30 02:38:31 -04:00
|
|
|
return throttle percentage from 0 to 100 for normal use and -100 to 100 when using reverse thrust
|
2014-08-03 04:16:51 -03:00
|
|
|
*/
|
2016-01-30 02:38:31 -04:00
|
|
|
int8_t Plane::throttle_percentage(void)
|
2014-08-03 04:16:51 -03:00
|
|
|
{
|
2016-05-11 02:14:43 -03:00
|
|
|
if (quadplane.in_vtol_mode()) {
|
2016-01-03 06:38:51 -04:00
|
|
|
return quadplane.throttle_percentage();
|
|
|
|
}
|
2014-08-03 04:16:51 -03:00
|
|
|
// to get the real throttle we need to use norm_output() which
|
|
|
|
// returns a number from -1 to 1.
|
2016-10-22 07:27:57 -03:00
|
|
|
float throttle = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
|
2016-02-09 01:46:56 -04:00
|
|
|
if (aparm.throttle_min >= 0) {
|
2017-01-08 19:20:38 -04:00
|
|
|
return constrain_int16(50*(throttle+1), 0, 100);
|
2016-02-09 01:46:56 -04:00
|
|
|
} else {
|
|
|
|
// reverse thrust
|
2016-10-22 07:27:57 -03:00
|
|
|
return constrain_int16(100*throttle, -100, 100);
|
2016-02-09 01:46:56 -04:00
|
|
|
}
|
2014-08-03 04:16:51 -03:00
|
|
|
}
|
2015-02-22 04:39:58 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
update AHRS soft arm state and log as needed
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::change_arm_state(void)
|
2015-02-22 04:39:58 -04:00
|
|
|
{
|
|
|
|
Log_Arm_Disarm();
|
2016-07-28 03:37:28 -03:00
|
|
|
update_soft_armed();
|
2015-11-24 04:24:04 -04:00
|
|
|
quadplane.set_armed(hal.util->get_soft_armed());
|
2015-02-22 04:39:58 -04:00
|
|
|
}
|
2015-03-16 20:09:37 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
arm motors
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::arm_motors(AP_Arming::ArmingMethod method)
|
2015-03-16 20:09:37 -03:00
|
|
|
{
|
|
|
|
if (!arming.arm(method)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
change_arm_state();
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
disarm motors
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::disarm_motors(void)
|
2015-03-16 20:09:37 -03:00
|
|
|
{
|
|
|
|
if (!arming.disarm()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (control_mode != AUTO) {
|
|
|
|
// reset the mission on disarm if we are not in auto
|
|
|
|
mission.reset();
|
|
|
|
}
|
|
|
|
|
|
|
|
// suppress the throttle in auto-throttle modes
|
|
|
|
throttle_suppressed = auto_throttle_mode;
|
|
|
|
|
|
|
|
//only log if disarming was successful
|
|
|
|
change_arm_state();
|
|
|
|
|
2016-04-27 01:36:13 -03:00
|
|
|
// reload target airspeed which could have been modified by a mission
|
2016-11-17 21:07:10 -04:00
|
|
|
plane.aparm.airspeed_cruise_cm.load();
|
2016-04-27 01:36:13 -03:00
|
|
|
|
2015-03-16 20:09:37 -03:00
|
|
|
return true;
|
2016-01-19 01:26:14 -04:00
|
|
|
}
|