ardupilot/ArduPlane/system.cpp

863 lines
23 KiB
C++
Raw Normal View History

2015-05-13 03:09:36 -03:00
#include "Plane.h"
#include "version.h"
2015-05-13 03:09:36 -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.
*
*****************************************************************************/
#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)
{
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"
" reboot reboot to flight mode\n"
"\n");
2012-08-21 23:19:51 -03:00
return(0);
}
// Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] = {
// 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)},
};
// Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands);
2015-05-13 03:09:36 -03:00
int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
{
2013-09-03 22:59:16 -03:00
hal.scheduler->reboot(false);
return 0;
}
// the user wants the CLI. It never exits
2015-05-13 03:09:36 -03:00
void Plane::run_cli(AP_HAL::UARTDriver *port)
{
// disable the failsafe code in the CLI
2016-10-28 19:10:03 -03:00
hal.scheduler->register_timer_failsafe(nullptr,1);
// disable the mavlink delay callback
2016-10-28 19:10:03 -03:00
hal.scheduler->register_delay_callback(nullptr, 5);
cliSerial = port;
Menu::set_port(port);
port->set_blocking_writes(true);
while (1) {
main_menu.run();
}
}
#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()
{
// initialise serial port
serial_manager.init_console();
cliSerial->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
//
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();
// initialise stats module
g2.stats.init();
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// set sensors to HIL mode
ins.set_hil_mode();
compass.set_hil_mode();
barometer.set_hil_mode();
}
#endif
set_control_channels();
#if HAVE_PX4_MIXER
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);
}
}
#endif
GCS_MAVLINK::set_dataflash(&DataFlash);
2014-01-19 21:58:49 -04:00
mavlink_system.sysid = g.sysid_this_mav;
// initialise serial ports
serial_manager.init();
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);
// setup any board specific drivers
BoardConfig.init();
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);
// 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);
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// initialise rangefinder
init_rangefinder();
// initialise battery monitoring
battery.init();
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();
2016-05-16 00:33:43 -03:00
// setup telem slots with serial ports
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
2016-05-16 00:33:43 -03:00
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
// setup frsky
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky, and pass a number of parameters to the library
frsky_telemetry.init(serial_manager, FIRMWARE_STRING,
MAV_TYPE_FIXED_WING,
&g.fs_batt_voltage, &g.fs_batt_mah);
#endif
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
// 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
if (g.hil_mode != 0) {
2015-08-23 09:21:51 -03:00
compass_ok = true;
}
2015-08-23 09:21:51 -03:00
#endif
if (!compass_ok) {
cliSerial->println("Compass initialisation failed!");
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
}
2012-08-21 23:19:51 -03:00
}
2015-01-02 20:09:02 -04:00
#if OPTFLOW == ENABLED
2015-01-02 20:09:02 -04:00
// make optflow available to libraries
if (optflow.enabled()) {
ahrs.set_optflow(&optflow);
}
#endif
2012-08-10 19:57:44 -03:00
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// GPS Initialization
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
2012-12-04 18:22:21 -04:00
relay.init();
#if MOUNT == ENABLED
// initialise camera mount
2016-01-05 01:23:47 -04:00
camera_mount.init(&DataFlash, serial_manager);
#endif
#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);
#endif
/*
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.
*/
2015-05-13 03:09:36 -03:00
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
if (g.cli_enabled == 1) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
cliSerial->println(msg);
2016-10-28 19:10:03 -03:00
if (gcs[1].initialised && (gcs[1].get_uart() != nullptr)) {
gcs[1].get_uart()->println(msg);
}
2016-10-28 19:10:03 -03:00
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) {
gcs[2].get_uart()->println(msg);
}
}
#endif // CLI_ENABLED
2015-07-31 19:39:36 -03:00
init_capabilities();
quadplane.setup();
startup_ground();
// don't initialise aux rc output until after quadplane is setup as
// that can change initial values of channels
init_rc_out_aux();
// 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);
2012-08-21 23:19:51 -03:00
// set the correct flight mode
// ---------------------------
reset_control_switch();
// initialise sensor
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
optflow.init();
}
#endif
}
//********************************************************************************
//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)
{
2016-08-13 04:54:37 -03:00
set_mode(INITIALISING, MODE_REASON_UNKNOWN);
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
2012-08-21 23:19:51 -03:00
#if (GROUND_START_DELAY > 0)
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With delay");
2012-08-21 23:19:51 -03:00
delay(GROUND_START_DELAY * 1000);
#endif
//INS ground start
2012-08-21 23:19:51 -03:00
//------------------------
//
2015-03-10 20:16:59 -03:00
startup_INS_ground();
2012-08-21 23:19:51 -03:00
// read the radio to set trims
// ---------------------------
if (g.trim_rc_at_start != 0) {
trim_radio();
}
2012-08-21 23:19:51 -03:00
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialise mission library
mission.init();
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
failsafe.last_heartbeat_ms = millis();
// 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
serial_manager.set_blocking_writes_all(false);
2015-05-06 23:11:52 -03:00
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_INFO,"Ready to fly");
}
2015-05-13 03:09:36 -03:00
enum FlightMode Plane::get_previous_mode() {
return previous_mode;
}
2016-08-13 04:54:37 -03:00
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
{
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;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
2016-11-18 18:05:45 -04:00
landing.commanded_go_around = false;
// not in pre-flare
landing.pre_flare = false;
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// reset external attitude guidance
guided_state.last_forced_rpy_ms.zero();
guided_state.last_forced_throttle_ms = 0;
// set mode
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
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// 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;
// start with previous WP at current location
prev_WP_loc = current_loc;
// new mode means new loiter
loiter.start_time_ms = 0;
// assume non-VTOL mode
auto_state.vtol_mode = false;
auto_state.vtol_loiter = false;
2012-08-21 23:19:51 -03:00
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
auto_navigation_mode = false;
break;
2012-08-21 23:19:51 -03:00
case MANUAL:
case STABILIZE:
case TRAINING:
2012-08-21 23:19:51 -03:00
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
auto_navigation_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
auto_navigation_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
auto_navigation_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
auto_navigation_mode = false;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
2012-08-21 23:19:51 -03:00
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
auto_navigation_mode = false;
set_target_altitude_current();
2012-08-21 23:19:51 -03:00
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
auto_navigation_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
2012-08-21 23:19:51 -03:00
case AUTO:
auto_throttle_mode = true;
auto_navigation_mode = true;
if (quadplane.available() && quadplane.enable == 2) {
auto_state.vtol_mode = true;
} else {
auto_state.vtol_mode = false;
}
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
2012-08-21 23:19:51 -03:00
break;
case RTL:
auto_throttle_mode = true;
auto_navigation_mode = true;
prev_WP_loc = current_loc;
do_RTL(get_RTL_altitude());
2012-08-21 23:19:51 -03:00
break;
case LOITER:
auto_throttle_mode = true;
auto_navigation_mode = true;
2012-08-21 23:19:51 -03:00
do_loiter_at_location();
break;
case AVOID_ADSB:
2012-08-21 23:19:51 -03:00
case GUIDED:
auto_throttle_mode = true;
auto_navigation_mode = true;
guided_throttle_passthru = false;
/*
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;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
auto_navigation_mode = false;
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
2012-08-21 23:19:51 -03:00
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
2016-07-10 21:23:08 -03:00
adsb.set_is_auto_mode(auto_navigation_mode);
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
2013-08-14 01:57:41 -03:00
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
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:
case AVOID_ADSB:
case GUIDED:
2014-10-01 01:19:20 -03:00
case AUTO:
case RTL:
case LOITER:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
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;
}
// 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)
{
// stop mission when we leave auto
if (mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND)
{
landing.restart_landing_sequence();
}
}
auto_state.started_flying_in_auto_ms = 0;
}
}
2015-05-13 03:09:36 -03:00
void Plane::check_long_failsafe()
{
uint32_t tnow = millis();
2012-08-21 23:19:51 -03:00
// only act on changes
// -------------------
2016-01-30 02:32:42 -04:00
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
if (failsafe.state == FAILSAFE_SHORT &&
(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);
} 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);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT &&
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);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
2014-03-18 18:34:35 -03:00
gcs[0].last_radio_status_remrssi_ms != 0 &&
(tnow - gcs[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
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;
}
2012-08-21 23:19:51 -03:00
}
}
2015-05-13 03:09:36 -03:00
void Plane::check_short_failsafe()
{
2012-08-21 23:19:51 -03:00
// only act on changes
// -------------------
2016-01-30 02:32:42 -04:00
if(failsafe.state == FAILSAFE_NONE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
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
}
}
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
}
}
}
2015-05-13 03:09:36 -03:00
void Plane::startup_INS_ground(void)
{
#if HIL_SUPPORT
if (g.hil_mode == 1) {
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
hal.scheduler->delay(1000);
}
}
#endif
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-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
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
2012-08-21 23:19:51 -03:00
// read Baro pressure at ground
//-----------------------------
2016-05-23 22:31:53 -03:00
init_barometer(true);
if (airspeed.enabled()) {
// initialize airspeed sensor
// --------------------------
2014-11-13 06:12:59 -04:00
zero_airspeed(true);
} else {
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_WARNING,"No airspeed");
}
}
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()
{
2013-08-29 00:13:58 -03:00
notify.update();
}
2015-05-13 03:09:36 -03:00
void Plane::resetPerfData(void)
{
perf.mainLoop_count = 0;
perf.G_Dt_max = 0;
perf.G_Dt_min = 0;
perf.num_long = 0;
perf.start_ms = millis();
perf.last_log_dropped = DataFlash.num_dropped();
}
2015-05-13 03:09:36 -03:00
void Plane::check_usb_mux(void)
{
bool usb_check = hal.gpio->usb_connected();
if (usb_check == usb_connected) {
return;
}
// the user has switched to/from the telemetry port
usb_connected = usb_check;
}
2015-05-13 03:09:36 -03:00
void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case MANUAL:
2015-10-25 16:22:31 -03:00
port->print("Manual");
break;
case CIRCLE:
2015-10-25 16:22:31 -03:00
port->print("Circle");
break;
case STABILIZE:
2015-10-25 16:22:31 -03:00
port->print("Stabilize");
break;
case TRAINING:
2015-10-25 16:22:31 -03:00
port->print("Training");
break;
case ACRO:
2015-10-25 16:22:31 -03:00
port->print("ACRO");
break;
case FLY_BY_WIRE_A:
2015-10-25 16:22:31 -03:00
port->print("FBW_A");
break;
case AUTOTUNE:
2015-10-25 16:22:31 -03:00
port->print("AUTOTUNE");
break;
case FLY_BY_WIRE_B:
2015-10-25 16:22:31 -03:00
port->print("FBW_B");
break;
case CRUISE:
2015-10-25 16:22:31 -03:00
port->print("CRUISE");
break;
case AUTO:
2015-10-25 16:22:31 -03:00
port->print("AUTO");
break;
case RTL:
2015-10-25 16:22:31 -03:00
port->print("RTL");
break;
case LOITER:
2015-10-25 16:22:31 -03:00
port->print("Loiter");
break;
case AVOID_ADSB:
port->print("AVOID_ADSB");
break;
case GUIDED:
2015-10-25 16:22:31 -03:00
port->print("Guided");
break;
2016-03-09 02:29:13 -04:00
case QSTABILIZE:
port->print("QStabilize");
break;
case QHOVER:
port->print("QHover");
break;
case QLOITER:
port->print("QLoiter");
break;
case QLAND:
port->print("QLand");
break;
case QRTL:
port->print("QRTL");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;
}
}
2012-09-18 00:48:08 -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
{
2015-10-25 16:22:31 -03:00
cliSerial->print(",");
2012-09-18 00:48:08 -03:00
}
#endif
2012-12-04 18:22:21 -04:00
/*
write to a servo
*/
2015-05-13 03:09:36 -03:00
void Plane::servo_write(uint8_t ch, uint16_t pwm)
{
#if HIL_SUPPORT
if (g.hil_mode==1 && !g.hil_servos) {
if (ch < 8) {
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
RC_Channel::rc_channel(ch)->set_radio_out(pwm);
}
return;
}
#endif
hal.rcout->enable_ch(ch);
hal.rcout->write(ch, pwm);
}
/*
should we log a message type now?
*/
2015-05-13 03:09:36 -03:00
bool Plane::should_log(uint32_t mask)
{
#if LOGGING_ENABLED == ENABLED
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
return false;
}
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;
#else
return false;
#endif
}
/*
return throttle percentage from 0 to 100 for normal use and -100 to 100 when using reverse thrust
*/
int8_t Plane::throttle_percentage(void)
{
if (quadplane.in_vtol_mode()) {
return quadplane.throttle_percentage();
}
// to get the real throttle we need to use norm_output() which
// returns a number from -1 to 1.
if (aparm.throttle_min >= 0) {
return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100);
} else {
// reverse thrust
return constrain_int16(100*channel_throttle->norm_output(), -100, 100);
}
}
/*
update AHRS soft arm state and log as needed
*/
2015-05-13 03:09:36 -03:00
void Plane::change_arm_state(void)
{
Log_Arm_Disarm();
update_soft_armed();
quadplane.set_armed(hal.util->get_soft_armed());
}
/*
arm motors
*/
2015-05-13 03:09:36 -03:00
bool Plane::arm_motors(AP_Arming::ArmingMethod method)
{
if (!arming.arm(method)) {
return false;
}
// only log if arming was successful
channel_throttle->enable_out();
change_arm_state();
return true;
}
/*
disarm motors
*/
2015-05-13 03:09:36 -03:00
bool Plane::disarm_motors(void)
{
if (!arming.disarm()) {
return false;
}
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
channel_throttle->disable_out();
}
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();
// 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();
return true;
}