ardupilot/ArduCopter/flight_mode.cpp

404 lines
11 KiB
C++
Raw Normal View History

2014-02-03 04:57:31 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
2014-02-03 04:57:31 -04:00
/*
* flight.pde - high level calls to set and update flight modes
* logic for individual flight modes is in control_acro.pde, control_stabilize.pde, etc
*/
// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
2016-05-12 14:24:23 -03:00
// returns true if mode was successfully set
2014-02-03 04:57:31 -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
2016-01-25 19:40:41 -04:00
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
2014-02-03 04:57:31 -04:00
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode
if (mode == control_mode) {
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
2016-01-25 19:40:41 -04:00
control_mode_reason = reason;
2014-02-03 04:57:31 -04:00
return true;
}
switch(mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
success = heli_acro_init(ignore_checks);
#else
success = acro_init(ignore_checks);
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
break;
case ALT_HOLD:
success = althold_init(ignore_checks);
break;
case AUTO:
success = auto_init(ignore_checks);
break;
case CIRCLE:
success = circle_init(ignore_checks);
break;
case LOITER:
success = loiter_init(ignore_checks);
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
case LAND:
success = land_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
case DRIFT:
success = drift_init(ignore_checks);
break;
case SPORT:
success = sport_init(ignore_checks);
break;
case FLIP:
success = flip_init(ignore_checks);
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = autotune_init(ignore_checks);
break;
#endif
2014-07-11 02:08:09 -03:00
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = poshold_init(ignore_checks);
2014-04-04 11:14:30 -03:00
break;
#endif
2014-04-04 11:14:30 -03:00
2015-05-17 00:22:20 -03:00
case BRAKE:
success = brake_init(ignore_checks);
2015-04-22 18:10:30 -03:00
break;
case THROW:
success = throw_init(ignore_checks);
break;
2014-02-03 04:57:31 -04:00
default:
success = false;
break;
}
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode, mode);
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
2016-01-25 19:40:41 -04:00
control_mode_reason = reason;
2016-01-25 19:50:12 -04:00
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
2014-02-03 04:57:31 -04:00
}else{
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
}
// update notify object
if (success) {
notify_flight_mode(control_mode);
}
2014-02-03 04:57:31 -04:00
// return success or failure
return success;
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Copter::update_flight_mode()
2014-02-03 04:57:31 -04:00
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
2014-02-03 04:57:31 -04:00
switch (control_mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
heli_acro_run();
#else
acro_run();
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
heli_stabilize_run();
#else
stabilize_run();
#endif
break;
case ALT_HOLD:
althold_run();
break;
case AUTO:
auto_run();
break;
case CIRCLE:
circle_run();
break;
case LOITER:
loiter_run();
break;
case GUIDED:
guided_run();
break;
case LAND:
land_run();
break;
case RTL:
rtl_run();
break;
case DRIFT:
drift_run();
break;
case SPORT:
sport_run();
break;
case FLIP:
flip_run();
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
break;
#endif
2014-04-04 11:14:30 -03:00
2014-07-11 02:08:09 -03:00
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
poshold_run();
2014-04-04 11:14:30 -03:00
break;
#endif
2015-04-22 18:10:30 -03:00
2015-05-17 00:22:20 -03:00
case BRAKE:
brake_run();
2015-04-22 18:10:30 -03:00
break;
case THROW:
throw_run();
break;
2016-01-26 21:30:13 -04:00
default:
break;
2014-02-03 04:57:31 -04:00
}
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
2016-01-25 17:26:20 -04:00
void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode)
2014-02-03 04:57:31 -04:00
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
2016-01-08 07:09:02 -04:00
if (old_control_mode == THROW) {
throw_exit();
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
ArduCopter: 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:46:59 -03:00
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in()));
}
2015-04-30 03:52:32 -03:00
// cancel any takeoffs in progress
takeoff_stop();
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control.use_flybar_passthrough(false, false);
motors.set_acro_tail(false);
}
// if we are changing from a mode that did not use manual throttle,
// stab col ramp value should be pre-loaded to the correct value to avoid a twitch
// heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
if (!mode_has_manual_throttle(old_control_mode)){
if (new_control_mode == STABILIZE){
input_manager.set_stab_col_ramp(1.0);
} else if (new_control_mode == ACRO){
input_manager.set_stab_col_ramp(0.0);
}
}
#endif //HELI_FRAME
2014-02-03 04:57:31 -04:00
}
// returns true or false whether mode requires GPS
2016-01-25 17:26:20 -04:00
bool Copter::mode_requires_GPS(control_mode_t mode) {
2014-02-03 04:57:31 -04:00
switch(mode) {
case AUTO:
case GUIDED:
case LOITER:
case RTL:
case CIRCLE:
case DRIFT:
2014-07-11 02:08:09 -03:00
case POSHOLD:
2015-05-17 00:22:20 -03:00
case BRAKE:
case THROW:
2014-02-03 04:57:31 -04:00
return true;
default:
return false;
}
return false;
}
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
2016-01-25 17:26:20 -04:00
bool Copter::mode_has_manual_throttle(control_mode_t mode) {
2014-02-03 04:57:31 -04:00
switch(mode) {
case ACRO:
case STABILIZE:
return true;
default:
return false;
}
return false;
}
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
2016-01-25 17:26:20 -04:00
bool Copter::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
}
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
2016-01-25 17:26:20 -04:00
void Copter::notify_flight_mode(control_mode_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case RTL:
case CIRCLE:
case LAND:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;
default:
// all other are manual flight modes
AP_Notify::flags.autopilot_mode = false;
break;
}
}
2014-02-03 04:57:31 -04:00
//
// print_flight_mode - prints flight mode to serial port.
//
void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
2014-02-03 04:57:31 -04:00
{
switch (mode) {
case STABILIZE:
2015-10-25 16:22:31 -03:00
port->print("STABILIZE");
2014-02-03 04:57:31 -04:00
break;
case ACRO:
2015-10-25 16:22:31 -03:00
port->print("ACRO");
2014-02-03 04:57:31 -04:00
break;
case ALT_HOLD:
2015-10-25 16:22:31 -03:00
port->print("ALT_HOLD");
2014-02-03 04:57:31 -04:00
break;
case AUTO:
2015-10-25 16:22:31 -03:00
port->print("AUTO");
2014-02-03 04:57:31 -04:00
break;
case GUIDED:
2015-10-25 16:22:31 -03:00
port->print("GUIDED");
2014-02-03 04:57:31 -04:00
break;
case LOITER:
2015-10-25 16:22:31 -03:00
port->print("LOITER");
2014-02-03 04:57:31 -04:00
break;
case RTL:
2015-10-25 16:22:31 -03:00
port->print("RTL");
2014-02-03 04:57:31 -04:00
break;
case CIRCLE:
2015-10-25 16:22:31 -03:00
port->print("CIRCLE");
2014-02-03 04:57:31 -04:00
break;
case LAND:
2015-10-25 16:22:31 -03:00
port->print("LAND");
2014-02-03 04:57:31 -04:00
break;
case DRIFT:
2015-10-25 16:22:31 -03:00
port->print("DRIFT");
2014-02-03 04:57:31 -04:00
break;
case SPORT:
2015-10-25 16:22:31 -03:00
port->print("SPORT");
2014-02-03 04:57:31 -04:00
break;
case FLIP:
2015-10-25 16:22:31 -03:00
port->print("FLIP");
2014-02-03 04:57:31 -04:00
break;
case AUTOTUNE:
2015-10-25 16:22:31 -03:00
port->print("AUTOTUNE");
2014-02-03 04:57:31 -04:00
break;
2014-07-11 02:08:09 -03:00
case POSHOLD:
2015-10-25 16:22:31 -03:00
port->print("POSHOLD");
2014-04-04 11:14:30 -03:00
break;
2015-05-17 00:22:20 -03:00
case BRAKE:
2015-10-25 16:22:31 -03:00
port->print("BRAKE");
2015-05-17 00:22:20 -03:00
break;
case THROW:
port->print("THROW");
break;
2014-02-03 04:57:31 -04:00
default:
port->printf("Mode(%u)", (unsigned)mode);
2014-02-03 04:57:31 -04:00
break;
}
}