ardupilot/ArduCopter/motors.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

228 lines
7.5 KiB
C++
Raw Normal View History

#include "Copter.h"
#define ARM_DELAY 20 // called at 10hz so 2 seconds
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
static uint32_t auto_disarm_begin;
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void Copter::arm_motors_check()
{
2012-08-21 23:19:50 -03:00
static int16_t arming_counter;
// check if arming/disarm using rudder is allowed
AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
arming_counter = 0;
return;
}
#if TOY_MODE_ENABLED
if (g2.toy_mode.enabled()) {
// not armed with sticks in toy mode
return;
}
#endif
2018-09-09 21:21:31 -03:00
// ensure throttle is down
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
if (channel_throttle->get_control_in() > 0) {
2012-08-21 23:19:50 -03:00
arming_counter = 0;
return;
}
2018-09-09 21:21:31 -03:00
int16_t yaw_in = channel_yaw->get_control_in();
2012-08-21 23:19:50 -03:00
// full right
2018-09-09 21:21:31 -03:00
if (yaw_in > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
2018-09-09 21:21:31 -03:00
if (arming_counter <= AUTO_TRIM_DELAY) {
arming_counter++;
}
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors->armed()) {
// reset arming counter if arming fail
if (!arming.arm(AP_Arming::Method::RUDDER)) {
arming_counter = 0;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && flightmode->mode_number() == Mode::Number::STABILIZE) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
auto_trim_counter = 250;
auto_trim_started = false;
// ensure auto-disarm doesn't trigger immediately
auto_disarm_begin = millis();
2012-08-21 23:19:50 -03:00
}
// full left and rudder disarming is enabled
} else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
if (!flightmode->has_manual_throttle() && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
2018-09-09 21:21:31 -03:00
if (arming_counter <= DISARM_DELAY) {
2012-08-21 23:19:50 -03:00
arming_counter++;
}
// disarm the motors
if (arming_counter == DISARM_DELAY && motors->armed()) {
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::RUDDER);
}
// Yaw is centered so reset arming counter
2018-09-09 21:21:31 -03:00
} else {
2012-08-21 23:19:50 -03:00
arming_counter = 0;
}
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void Copter::auto_disarm_check()
{
uint32_t tnow_ms = millis();
uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors->armed() || disarm_delay_ms == 0 || flightmode->mode_number() == Mode::Number::THROW) {
auto_disarm_begin = tnow_ms;
return;
}
// if the rotor is still spinning, don't initiate auto disarm
if (motors->get_spool_state() > AP_Motors::SpoolState::GROUND_IDLE) {
auto_disarm_begin = tnow_ms;
return;
}
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) {
#if FRAME_CONFIG != HELI_FRAME
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay_ms /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (flightmode->has_manual_throttle() || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = get_throttle_mid() + g.throttle_deadzone;
2017-01-06 21:06:40 -04:00
thr_low = channel_throttle->get_control_in() <= deadband_top;
}
if (!thr_low || !ap.land_complete) {
// reset timer
auto_disarm_begin = tnow_ms;
}
}
// disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::DISARMDELAY);
auto_disarm_begin = tnow_ms;
}
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
Copter: run copter attitude control with separate rate thread run motors output at rate thread loop rate allow rate thread to be enabled/disabled at runtime for in-flight impact testing setup the right PID notch sample rate when using the rate thread the PID notches run at a very different sample rate call update_dynamic_notch_at_specified_rate() in rate thread log RTDT messages to track rate loop performance set dt each cycle of the rate loop thread run rate controller on samples as soon as they are ready detect overload conditions in both the rate loop and main loop decimate the rate thread if the CPU appears overloaded decimate the gyro window inside the IMU add in gyro drift to attitude rate thread add fixed-rate thread option configure rate loop based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED better rate loop thread decimation management ensure fix rate attitude is enabled on arming add rate loop timing debug update backend filters rather than all the backends provide more options around attitude rates only log attitude and IMU from main loop force trigger_groups() and reduce attitude thread priority migrate fast rate enablement to FSTRATE_ENABLE remove rate thread logging configuration and choose sensible logging rates conditionally compile rate thread pieces allow fast rate decimation to be user throttled if target rate changes immediately jump to target rate recover quickly from rate changes ensure fixed rate always prints the rate on arming and is always up to date add support for fixed rate attitude that does not change when disarmed only push to subsystems at main loop rate add logging and motor timing debug correctly round gyro decimation rates set dshot rate when changing attitude rate fallback to higher dshot rates at lower loop rates re-factor rate loop rate updates log rates in systemid mode reset target modifiers at loop rate don't compile in support on tradheli move rate thread into its own compilation unit add rate loop config abstraction that allows code to be elided on non-copter builds dynamically enable/disable rate thread correctly add design comment for the rate thread Co-authored-by: Andrew Tridgell <andrew@tridgell.net>
2024-02-11 01:19:56 -04:00
void Copter::motors_output(bool full_push)
{
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
if (g2.afs.should_crash_vehicle()) {
g2.afs.terminate_vehicle();
if (!g2.afs.terminating_vehicle_via_landing()) {
return;
}
// landing must continue to run the motors output
}
#endif
2016-01-21 22:59:47 -04:00
// Update arming delay state
if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || flightmode->mode_number() == Mode::Number::THROW)) {
2016-01-21 22:59:47 -04:00
ap.in_arming_delay = false;
}
// output any servo channels
SRV_Channels::calc_pwm();
auto &srv = AP::srv();
// cork now, so that all channel outputs happen at once
srv.cork();
// update output on any aux channels, for manual passthru
SRV_Channels::output_ch_all();
// update motors interlock state
bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop();
if (!motors->get_interlock() && interlock) {
motors->set_interlock(true);
LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_ENABLED);
} else if (motors->get_interlock() && !interlock) {
motors->set_interlock(false);
LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_DISABLED);
}
if (ap.motor_test) {
// check if we are performing the motor test
motor_test_output();
} else {
2016-01-21 22:59:47 -04:00
// send output signals to motors
flightmode->output_to_motors();
}
// push all channels
Copter: run copter attitude control with separate rate thread run motors output at rate thread loop rate allow rate thread to be enabled/disabled at runtime for in-flight impact testing setup the right PID notch sample rate when using the rate thread the PID notches run at a very different sample rate call update_dynamic_notch_at_specified_rate() in rate thread log RTDT messages to track rate loop performance set dt each cycle of the rate loop thread run rate controller on samples as soon as they are ready detect overload conditions in both the rate loop and main loop decimate the rate thread if the CPU appears overloaded decimate the gyro window inside the IMU add in gyro drift to attitude rate thread add fixed-rate thread option configure rate loop based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED better rate loop thread decimation management ensure fix rate attitude is enabled on arming add rate loop timing debug update backend filters rather than all the backends provide more options around attitude rates only log attitude and IMU from main loop force trigger_groups() and reduce attitude thread priority migrate fast rate enablement to FSTRATE_ENABLE remove rate thread logging configuration and choose sensible logging rates conditionally compile rate thread pieces allow fast rate decimation to be user throttled if target rate changes immediately jump to target rate recover quickly from rate changes ensure fixed rate always prints the rate on arming and is always up to date add support for fixed rate attitude that does not change when disarmed only push to subsystems at main loop rate add logging and motor timing debug correctly round gyro decimation rates set dshot rate when changing attitude rate fallback to higher dshot rates at lower loop rates re-factor rate loop rate updates log rates in systemid mode reset target modifiers at loop rate don't compile in support on tradheli move rate thread into its own compilation unit add rate loop config abstraction that allows code to be elided on non-copter builds dynamically enable/disable rate thread correctly add design comment for the rate thread Co-authored-by: Andrew Tridgell <andrew@tridgell.net>
2024-02-11 01:19:56 -04:00
if (full_push) {
srv.push();
} else {
hal.rcout->push();
}
}
// motors_output from main thread
void Copter::motors_output_main()
{
if (!using_rate_thread) {
motors_output();
}
}
// check for pilot stick input to trigger lost vehicle alarm
void Copter::lost_vehicle_check()
{
static uint8_t soundalarm_counter;
// disable if aux switch is setup to vehicle alarm as the two could interfere
if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::LOST_VEHICLE_SOUND)) {
return;
}
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
if (ap.throttle_zero && !motors->armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
if (AP_Notify::flags.vehicle_lost == false) {
AP_Notify::flags.vehicle_lost = true;
gcs().send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm");
}
} else {
soundalarm_counter++;
}
} else {
soundalarm_counter = 0;
if (AP_Notify::flags.vehicle_lost == true) {
AP_Notify::flags.vehicle_lost = false;
}
}
}