ardupilot/ArduCopter/radio.cpp

212 lines
7.4 KiB
C++
Raw Normal View History

#include "Copter.h"
// Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
2012-08-04 13:39:20 -03:00
void Copter::default_dead_zones()
{
channel_roll->set_default_dead_zone(20);
channel_pitch->set_default_dead_zone(20);
2012-08-16 21:50:02 -03:00
#if FRAME_CONFIG == HELI_FRAME
channel_throttle->set_default_dead_zone(10);
channel_yaw->set_default_dead_zone(15);
2012-08-16 21:50:02 -03:00
#else
channel_throttle->set_default_dead_zone(30);
channel_yaw->set_default_dead_zone(20);
2012-08-16 21:50:02 -03:00
#endif
rc().channel(CH_6)->set_default_dead_zone(0);
}
void Copter::init_rc_in()
{
channel_roll = rc().channel(rcmap.roll()-1);
channel_pitch = rc().channel(rcmap.pitch()-1);
channel_throttle = rc().channel(rcmap.throttle()-1);
channel_yaw = rc().channel(rcmap.yaw()-1);
2012-08-16 21:50:02 -03:00
// set rc channel ranges
channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
2017-01-06 21:06:40 -04:00
channel_throttle->set_range(1000);
2012-08-16 21:50:02 -03:00
// set default dead zones
default_dead_zones();
// initialise throttle_zero flag
ap.throttle_zero = true;
}
// init_rc_out -- initialise motors
void Copter::init_rc_out()
{
motors->set_loop_rate(scheduler.get_loop_rate_hz());
motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
2017-04-17 06:00:26 -03:00
// enable aux servos to cope with multiple output channels per motor
SRV_Channels::enable_aux_servos();
// update rate must be set after motors->init() to allow for motor mapping
motors->set_update_rate(g.rc_speed);
#if FRAME_CONFIG != HELI_FRAME
motors->set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#else
2019-10-17 03:03:15 -03:00
// setup correct scaling for ESCs like the UAVCAN ESCs which
// take a proportion of speed.
hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
2012-08-16 21:50:02 -03:00
// refresh auxiliary channel to function map
2017-01-06 21:06:40 -04:00
SRV_Channels::update_aux_servo_function();
#if FRAME_CONFIG != HELI_FRAME
/*
setup a default safety ignore mask, so that servo gimbals can be active while safety is on
*/
uint16_t safety_ignore_mask = (~copter.motors->get_motor_mask()) & 0x3FFF;
BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask);
#endif
}
// enable_motor_output() - enable and output lowest possible value to motors
void Copter::enable_motor_output()
{
2012-08-16 21:50:02 -03:00
// enable motors
motors->output_min();
}
void Copter::read_radio()
{
const uint32_t tnow_ms = millis();
2014-10-07 11:01:15 -03:00
if (rc().read_input()) {
ap.new_radio_frame = true;
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_throttle_and_failsafe(channel_throttle->get_radio_in());
set_throttle_zero_flag(channel_throttle->get_control_in());
// RC receiver must be attached if we've just got input
ap.rc_receiver_present = true;
// pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
radio_passthrough_to_motors();
2018-08-04 08:32:31 -03:00
const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
2017-01-06 21:06:40 -04:00
rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
last_radio_update_ms = tnow_ms;
2018-08-04 08:32:31 -03:00
return;
}
// No radio input this time
if (failsafe.radio) {
// already in failsafe!
return;
}
const uint32_t elapsed = tnow_ms - last_radio_update_ms;
// turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS;
if (elapsed < timeout) {
// not timed out yet
return;
}
if (!g.failsafe_throttle) {
// throttle failsafe not enabled
return;
2012-08-16 21:50:02 -03:00
}
2018-08-04 08:32:31 -03:00
if (!ap.rc_receiver_present && !motors->armed()) {
// we only failsafe if we are armed OR we have ever seen an RC receiver
return;
}
// Nobody ever talks to us. Log an error and enter failsafe.
AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
2018-08-04 08:32:31 -03:00
set_failsafe_radio(true);
}
#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
{
// if failsafe not enabled pass through throttle and exit
if(g.failsafe_throttle == FS_THR_DISABLED) {
2012-08-16 21:50:02 -03:00
return;
}
2012-08-16 21:50:02 -03:00
//check for low throttle value
if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
// if we are already in failsafe or motors not armed pass through throttle and exit
if (failsafe.radio || !(ap.rc_receiver_present || motors->armed())) {
return;
2012-08-16 21:50:02 -03:00
}
// check for 3 low throttle values
2016-05-12 14:24:23 -03:00
// Note: we do not pass through the low throttle until 3 low throttle values are received
failsafe.radio_counter++;
if( failsafe.radio_counter >= FS_COUNTER ) {
failsafe.radio_counter = FS_COUNTER; // check to ensure we don't overflow the counter
set_failsafe_radio(true);
2012-08-16 21:50:02 -03:00
}
}else{
// we have a good throttle so reduce failsafe counter
failsafe.radio_counter--;
if( failsafe.radio_counter <= 0 ) {
failsafe.radio_counter = 0; // check to ensure we don't underflow the counter
// disengage failsafe after three (nearly) consecutive valid throttle values
if (failsafe.radio) {
set_failsafe_radio(false);
}
2012-08-16 21:50:02 -03:00
}
// pass through throttle
2012-08-16 21:50:02 -03:00
}
}
#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control
// throttle_zero is used to determine if the pilot intends to shut down the motors
// Basically, this signals when we are not flying. We are either on the ground
// or the pilot has shut down the copter in the air and it is free-falling
void Copter::set_throttle_zero_flag(int16_t throttle_control)
{
static uint32_t last_nonzero_throttle_ms = 0;
uint32_t tnow_ms = millis();
// if not using throttle interlock and non-zero throttle and not E-stopped,
// or using motor interlock and it's enabled, then motors are running,
// and we are flying. Immediately set as non-zero
if ((!ap.using_interlock && (throttle_control > 0) && !SRV_Channels::get_emergency_stop()) ||
(ap.using_interlock && motors->get_interlock()) ||
ap.armed_with_airmode_switch || air_mode == AirMode::AIRMODE_ENABLED) {
last_nonzero_throttle_ms = tnow_ms;
ap.throttle_zero = false;
} else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
ap.throttle_zero = true;
}
2015-03-16 01:35:57 -03:00
}
// pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax copters)
void Copter::radio_passthrough_to_motors()
{
motors->set_radio_passthrough(channel_roll->norm_input(),
channel_pitch->norm_input(),
channel_throttle->get_control_in_zero_dz()*0.001f,
channel_yaw->norm_input());
}
/*
return the throttle input for mid-stick as a control-in value
*/
int16_t Copter::get_throttle_mid(void)
{
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
return g2.toy_mode.get_throttle_mid();
}
#endif
return channel_throttle->get_control_mid();
}