/*
initialise RC output channels for aux channels
*/
void Plane::init_rc_out_aux()
{
SRV_Channels::enable_aux_servos();
SRV_Channels::cork();
servos_output();
// setup PWM values to send if the FMU firmware dies
// allows any VTOL motors to shut off
SRV_Channels::setup_failsafe_trim_all_non_motors();
}
/*
initialise RC output channels for aux channels
*/
void Plane::init_rc_out_aux()
{
update_aux();
SRV_Channels::enable_aux_servos();
SRV_Channels::cork();
// Initialization of servo outputs
SRV_Channels::output_trim_all();
servos_output();
// setup PWM values to send if the FMU firmware dies
SRV_Channels::setup_failsafe_trim_all();
}
/*
Set the flight control servos based on the current calculated values
This function operates by first building up output values for
channels using set_servo() and set_radio_out(). Using
set_radio_out() is for when a raw PWM value of output is given which
does not depend on any output scaling. Using set_servo() is for when
scaling and mixing will be needed.
Finally servos_output() is called to push the final PWM values
for output channels
*/
void Plane::set_servos(void)
{
// start with output corked. the cork is released when we run
// servos_output(), which is run from all code paths in this
// function
SRV_Channels::cork();
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
if (afs.should_crash_vehicle()) {
afs.terminate_vehicle();
return;
}
// do any transition updates for quadplane
quadplane.update();
if (control_mode == AUTO && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
servos_output();
return;
}
/*
see if we are doing ground steering.
*/
if (!steering_control.ground_steering) {
// we are not at an altitude for ground steering. Set the nose
// wheel to the rudder just in case the barometer has drifted
// a lot
steering_control.steering = steering_control.rudder;
} else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
// we are within the ground steering altitude but don't have a
// dedicated steering channel. Set the rudder to the ground
// steering output
steering_control.rudder = steering_control.steering;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
if (control_mode == TRAINING) {
steering_control.rudder = channel_rudder->get_control_in();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
set_servos_manual_passthrough();
} else {
set_servos_controlled();
}
// setup flap outputs
set_servos_flaps();
if (auto_throttle_mode ||
quadplane.in_assisted_flight() ||
quadplane.in_vtol_mode()) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit();
}
if (!arming.is_armed()) {
//Some ESCs get noisy (beep error msgs) if PWM == 0.
//This little segment aims to avoid this.
switch (arming.arming_required()) {
case AP_Arming::NO:
//keep existing behavior: do nothing to radio_out
//(don't disarm throttle channel even if AP_Arming class is)
break;
case AP_Arming::YES_ZERO_PWM:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
break;
case AP_Arming::YES_MIN_PWM:
default:
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
break;
}
}
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// get the servos to the GCS immediately for HIL
if (HAVE_PAYLOAD_SPACE(MAVLINK_COMM_0, RC_CHANNELS_SCALED)) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
// we don't run the output mixer
return;
}
}
#endif
if (landing.get_then_servos_neutral() > 0 &&
control_mode == AUTO &&
landing.get_disarm_delay() > 0 &&
landing.is_complete() &&
!arming.is_armed()) {
// after an auto land and auto disarm, set the servos to be neutral just
// in case we're upside down or some crazy angle and straining the servos.
if (landing.get_then_servos_neutral() == 1) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
} else if (landing.get_then_servos_neutral() == 2) {
SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
}
uint8_t override_pct;
if (g2.ice_control.throttle_override(override_pct)) {
// the ICE controller wants to override the throttle for starting
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct);
}
// run output mixer and send values to the hal for output
servos_output();
}
/*
* this failsafe_check function is called from the core timer interrupt
* at 1kHz.
*/
void Plane::failsafe_check(void)
{
static uint16_t last_mainLoop_count;
static uint32_t last_timestamp;
static bool in_failsafe;
uint32_t tnow = micros();
if (perf.mainLoop_count != last_mainLoop_count) {
// the main loop is running, all is OK
last_mainLoop_count = perf.mainLoop_count;
last_timestamp = tnow;
in_failsafe = false;
return;
}
if (tnow - last_timestamp > 200000) {
// we have gone at least 0.2 seconds since the main loop
// ran. That means we're in trouble, or perhaps are in
// an initialisation routine or log erase. Start passing RC
// inputs through to outputs
in_failsafe = true;
}
if (in_failsafe && tnow - last_timestamp > 20000) {
last_timestamp = tnow;
if (in_calibration) {
// tell the failsafe system that we are calibrating
// sensors, so don't trigger failsafe
afs.heartbeat();
}
if (hal.rcin->num_channels() < 5) {
// we don't have any RC input to pass through
return;
}
// pass RC inputs to outputs every 20ms
hal.rcin->clear_overrides();
int16_t roll = channel_roll->get_control_in_zero_dz();
int16_t pitch = channel_pitch->get_control_in_zero_dz();
int16_t throttle = channel_throttle->get_control_in_zero_dz();
int16_t rudder = channel_rudder->get_control_in_zero_dz();
if (!hal.util->get_soft_armed()) {
throttle = 0;
}
// setup secondary output channels that don't have
// corresponding input channels
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
if (afs.should_crash_vehicle()) {
afs.terminate_vehicle();
return;
}
// setup secondary output channels that do have
// corresponding input channels
SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0);
// setup flaperons
flaperon_update(0);
servos_output();
}
}
