mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
6f200fa923
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)
404 lines
11 KiB
C++
404 lines
11 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include "Copter.h"
|
|
|
|
/*
|
|
* 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)
|
|
// returns true if mode was succesfully set
|
|
// 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
|
|
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
|
{
|
|
// 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;
|
|
|
|
control_mode_reason = reason;
|
|
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
|
|
|
|
#if POSHOLD_ENABLED == ENABLED
|
|
case POSHOLD:
|
|
success = poshold_init(ignore_checks);
|
|
break;
|
|
#endif
|
|
|
|
case BRAKE:
|
|
success = brake_init(ignore_checks);
|
|
break;
|
|
|
|
case THROW:
|
|
success = throw_init(ignore_checks);
|
|
break;
|
|
|
|
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;
|
|
control_mode_reason = reason;
|
|
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
|
|
}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);
|
|
}
|
|
|
|
// 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()
|
|
{
|
|
// Update EKF speed limit - used to limit speed when we are using optical flow
|
|
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
|
|
|
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
|
|
|
|
#if POSHOLD_ENABLED == ENABLED
|
|
case POSHOLD:
|
|
poshold_run();
|
|
break;
|
|
#endif
|
|
|
|
case BRAKE:
|
|
brake_run();
|
|
break;
|
|
|
|
case THROW:
|
|
throw_run();
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
// exit_mode - high level call to organise cleanup as a flight mode is exited
|
|
void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode)
|
|
{
|
|
#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
|
|
}
|
|
|
|
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
|
|
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in()));
|
|
}
|
|
|
|
// 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
|
|
}
|
|
|
|
// returns true or false whether mode requires GPS
|
|
bool Copter::mode_requires_GPS(control_mode_t mode) {
|
|
switch(mode) {
|
|
case AUTO:
|
|
case GUIDED:
|
|
case LOITER:
|
|
case RTL:
|
|
case CIRCLE:
|
|
case DRIFT:
|
|
case POSHOLD:
|
|
case BRAKE:
|
|
case THROW:
|
|
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)
|
|
bool Copter::mode_has_manual_throttle(control_mode_t mode) {
|
|
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
|
|
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
|
|
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;
|
|
}
|
|
}
|
|
|
|
//
|
|
// print_flight_mode - prints flight mode to serial port.
|
|
//
|
|
void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
|
{
|
|
switch (mode) {
|
|
case STABILIZE:
|
|
port->print("STABILIZE");
|
|
break;
|
|
case ACRO:
|
|
port->print("ACRO");
|
|
break;
|
|
case ALT_HOLD:
|
|
port->print("ALT_HOLD");
|
|
break;
|
|
case AUTO:
|
|
port->print("AUTO");
|
|
break;
|
|
case GUIDED:
|
|
port->print("GUIDED");
|
|
break;
|
|
case LOITER:
|
|
port->print("LOITER");
|
|
break;
|
|
case RTL:
|
|
port->print("RTL");
|
|
break;
|
|
case CIRCLE:
|
|
port->print("CIRCLE");
|
|
break;
|
|
case LAND:
|
|
port->print("LAND");
|
|
break;
|
|
case DRIFT:
|
|
port->print("DRIFT");
|
|
break;
|
|
case SPORT:
|
|
port->print("SPORT");
|
|
break;
|
|
case FLIP:
|
|
port->print("FLIP");
|
|
break;
|
|
case AUTOTUNE:
|
|
port->print("AUTOTUNE");
|
|
break;
|
|
case POSHOLD:
|
|
port->print("POSHOLD");
|
|
break;
|
|
case BRAKE:
|
|
port->print("BRAKE");
|
|
break;
|
|
case THROW:
|
|
port->print("THROW");
|
|
break;
|
|
default:
|
|
port->printf("Mode(%u)", (unsigned)mode);
|
|
break;
|
|
}
|
|
}
|
|
|