Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-04 15:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity
3dcf4cfb55
ardupilot/ArduPlane/system.cpp
Andrew Tridgell 3dcf4cfb55 Plane: removed resetting integrators on mode change 
this removes the zero of roll, pitch and yaw integrator on mode
change, now only doing the steering integrator.

The zeroing was from the early days when our controllers were not as
good. It should not be needed any more.
2016-12-14 20:22:56 +11:00

860 lines
22 KiB
C++
Raw Blame History

#include "Plane.h"
#include "version.h"
/*****************************************************************************
* 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
int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Commands:\n"
" logs log readback/setup mode\n"
" setup setup mode\n"
" test test mode\n"
" reboot reboot to flight mode\n"
"\n");
return(0);
}
// Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] = {
// command function called
// ======= ===============
{"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);
int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
{
hal.scheduler->reboot(false);
return 0;
}
// the user wants the CLI. It never exits
void Plane::run_cli(AP_HAL::UARTDriver *port)
{
// disable the failsafe code in the CLI
hal.scheduler->register_timer_failsafe(nullptr,1);
// disable the mavlink delay callback
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
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());
//
// Check the EEPROM format version before loading any parameters from EEPROM
//
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);
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();
// 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();
// 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 = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
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
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// initialise airspeed sensor
airspeed.init();
if (g.compass_enabled==true) {
bool compass_ok = compass.init() && compass.read();
#if HIL_SUPPORT
if (g.hil_mode != 0) {
compass_ok = true;
}
#endif
if (!compass_ok) {
cliSerial->println("Compass initialisation failed!");
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
}
}
#if OPTFLOW == ENABLED
// make optflow available to libraries
if (optflow.enabled()) {
ahrs.set_optflow(&optflow);
}
#endif
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// GPS Initialization
gps.init(&DataFlash, serial_manager);
init_rc_in(); // sets up rc channels from radio
relay.init();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(&DataFlash, serial_manager);
#endif
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
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);
if (gcs[1].initialised && (gcs[1].get_uart() != nullptr)) {
gcs[1].get_uart()->println(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) {
gcs[2].get_uart()->println(msg);
}
}
#endif // CLI_ENABLED
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();
set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN);
// 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
//********************************************************************************
void Plane::startup_ground(void)
{
set_mode(INITIALISING, MODE_REASON_UNKNOWN);
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
#if (GROUND_START_DELAY > 0)
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With delay");
delay(GROUND_START_DELAY * 1000);
#endif
//INS ground start
//------------------------
//
startup_INS_ground();
// read the radio to set trims
// ---------------------------
if (g.trim_rc_at_start != 0) {
trim_radio();
}
// 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);
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
ins.set_dataflash(&DataFlash);
gcs_send_text(MAV_SEVERITY_INFO,"Ready to fly");
}
enum FlightMode Plane::get_previous_mode() {
return previous_mode;
}
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
{
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
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;
control_mode = mode;
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
#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;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
auto_navigation_mode = false;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
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;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
auto_navigation_mode = false;
set_target_altitude_current();
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;
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();
break;
case RTL:
auto_throttle_mode = true;
auto_navigation_mode = true;
prev_WP_loc = current_loc;
do_RTL(get_RTL_altitude());
break;
case LOITER:
auto_throttle_mode = true;
auto_navigation_mode = true;
do_loiter_at_location();
break;
case AVOID_ADSB:
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;
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;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
adsb.set_is_auto_mode(auto_navigation_mode);
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset steering integrator on mode change
steerController.reset_I();
}
/*
set_mode() wrapper for MAVLink SET_MODE
*/
bool Plane::mavlink_set_mode(uint8_t mode)
{
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:
case AUTO:
case RTL:
case LOITER:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
return true;
}
return false;
}
// exit_mode - perform any cleanup required when leaving a flight mode
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;
}
}
void Plane::check_long_failsafe()
{
uint32_t tnow = millis();
// only act on changes
// -------------------
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) {
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) {
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) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
gcs[0].last_radio_status_remrssi_ms != 0 &&
(tnow - gcs[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
}
} 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;
}
}
}
void Plane::check_short_failsafe()
{
// only act on changes
// -------------------
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) {
// The condition is checked and the flag ch3_failsafe is set in radio.cpp
if(failsafe.ch3_failsafe) {
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);
}
}
if(failsafe.state == FAILSAFE_SHORT) {
if(!failsafe.ch3_failsafe) {
failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE);
}
}
}
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) {
gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane");
hal.scheduler->delay(100);
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_wind_estimation(true);
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
// read Baro pressure at ground
//-----------------------------
init_barometer(true);
if (airspeed.enabled()) {
// initialize airspeed sensor
// --------------------------
zero_airspeed(true);
} else {
gcs_send_text(MAV_SEVERITY_WARNING,"No airspeed");
}
}
// updates the status of the notify objects
// should be called at 50hz
void Plane::update_notify()
{
notify.update();
}
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();
}
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;
}
void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case MANUAL:
port->print("Manual");
break;
case CIRCLE:
port->print("Circle");
break;
case STABILIZE:
port->print("Stabilize");
break;
case TRAINING:
port->print("Training");
break;
case ACRO:
port->print("ACRO");
break;
case FLY_BY_WIRE_A:
port->print("FBW_A");
break;
case AUTOTUNE:
port->print("AUTOTUNE");
break;
case FLY_BY_WIRE_B:
port->print("FBW_B");
break;
case CRUISE:
port->print("CRUISE");
break;
case AUTO:
port->print("AUTO");
break;
case RTL:
port->print("RTL");
break;
case LOITER:
port->print("Loiter");
break;
case AVOID_ADSB:
port->print("AVOID_ADSB");
break;
case GUIDED:
port->print("Guided");
break;
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;
}
}
#if CLI_ENABLED == ENABLED
void Plane::print_comma(void)
{
cliSerial->print(",");
}
#endif
/*
write to a servo
*/
void Plane::servo_write(uint8_t ch, uint16_t pwm)
{
#if HIL_SUPPORT
if (g.hil_mode==1 && !g.hil_servos) {
if (ch < 8) {
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?
*/
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();
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
*/
void Plane::change_arm_state(void)
{
Log_Arm_Disarm();
update_soft_armed();
quadplane.set_armed(hal.util->get_soft_armed());
}
/*
arm motors
*/
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
*/
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
plane.aparm.airspeed_cruise_cm.load();
return true;
}
Reference in New Issue View Git Blame Copy Permalink