/*****************************************
* Throttle slew limit
*****************************************/
void Plane::throttle_slew_limit(int16_t last_throttle)
{
uint8_t slewrate = aparm.throttle_slewrate;
if (control_mode==AUTO) {
if (auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
slewrate = g.takeoff_throttle_slewrate;
} else if (g.land_throttle_slewrate != 0 &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE)) {
slewrate = g.land_throttle_slewrate;
}
}
// if slew limit rate is set to zero then do not slew limit
if (slewrate) {
// limit throttle change by the given percentage per second
float temp = slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
// allow a minimum change of 1 PWM per cycle
if (temp < 1) {
temp = 1;
}
channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp));
}
}
/*
calculate yaw control for ground steering
*/
void Plane::calc_nav_yaw_ground(void)
{
if (gps.ground_speed() < 1 &&
channel_throttle->get_control_in() == 0 &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// manual rudder control while still
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
steering_control.steering = rudder_input;
return;
}
float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
steer_rate = 0;
}
if (!is_zero(steer_rate)) {
// pilot is giving rudder input
steer_state.locked_course = false;
} else if (!steer_state.locked_course) {
// pilot has released the rudder stick or we are still - lock the course
steer_state.locked_course = true;
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
steer_state.locked_course_err = 0;
}
}
if (!steer_state.locked_course) {
// use a rate controller at the pilot specified rate
steering_control.steering = steerController.get_steering_out_rate(steer_rate);
} else {
// use a error controller on the summed error
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}
// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{
// swash servo initialisation
_servo_1.set_range(0,1000);
_servo_2.set_range(0,1000);
_servo_3.set_range(0,1000);
_servo_4.set_angle(4500);
// range check collective min, max and mid
if( _collective_min >= _collective_max ) {
_collective_min = 1000;
_collective_max = 2000;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
// calculate collective mid point as a number from 0 to 1000
_collective_mid_pwm = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min))*1000.0f;
// determine roll, pitch and collective input scaling
_roll_scaler = (float)_roll_max/4500.0f;
_pitch_scaler = (float)_pitch_max/4500.0f;
_collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
// servo min/max values
_servo_1.radio_min = 1000;
_servo_1.radio_max = 2000;
_servo_2.radio_min = 1000;
_servo_2.radio_max = 2000;
_servo_3.radio_min = 1000;
_servo_3.radio_max = 2000;
// mark swash as initialised
_heliflags.swash_initialised = true;
}
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Copter::get_throttle_pre_takeoff(float input_thr)
{
// exit immediately if input_thr is zero
if (input_thr <= 0.0f) {
return 0.0f;
}
// TODO: does this parameter sanity check really belong here?
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
float in_min = g.throttle_min;
float in_max = get_takeoff_trigger_throttle();
#if FRAME_CONFIG == HELI_FRAME
// helicopters swash will move from bottom to 1/2 of mid throttle
float out_min = 0;
#else
// multicopters will output between spin-when-armed and 1/2 of mid throttle
float out_min = motors.get_throttle_warn();
#endif
float out_max = get_non_takeoff_throttle();
if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
in_min = channel_throttle->get_control_mid();
}
float input_range = in_max-in_min;
float output_range = out_max-out_min;
// sanity check ranges
if (input_range <= 0.0f || output_range <= 0.0f) {
return 0.0f;
}
return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
}
// calculate pilot input to nudge speed up or down
// target_speed should be in meters/sec
// cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
// return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle)
{
// return immediately during RC/GCS failsafe
if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
return target_speed;
}
// return immediately if pilot is not attempting to nudge speed
// pilot can nudge up speed if throttle (in range -100 to +100) is above 50% of center in direction of travel
const int16_t pilot_throttle = constrain_int16(rover.channel_throttle->get_control_in(), -100, 100);
if (((pilot_throttle <= 50) && (target_speed >= 0.0f)) ||
((pilot_throttle >= -50) && (target_speed <= 0.0f))) {
return target_speed;
}
// sanity checks
if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
return target_speed;
}
// project vehicle's maximum speed
const float vehicle_speed_max = calc_speed_max(cruise_speed, cruise_throttle);
// return unadjusted target if already over vehicle's projected maximum speed
if (fabsf(target_speed) >= vehicle_speed_max) {
return target_speed;
}
const float speed_increase_max = vehicle_speed_max - fabsf(target_speed);
float speed_nudge = ((static_cast<float>(abs(pilot_throttle)) - 50.0f) * 0.02f) * speed_increase_max;
if (pilot_throttle < 0) {
speed_nudge = -speed_nudge;
}
return target_speed + speed_nudge;
}
/*
calculate yaw control for coordinated flight
*/
void Plane::calc_nav_yaw_coordinated(float speed_scaler)
{
bool disable_integrator = false;
if (control_mode == STABILIZE && rudder_input != 0) {
disable_integrator = true;
}
int16_t commanded_rudder;
// Received an external msg that guides yaw in the last 3 seconds?
if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
plane.guided_state.last_forced_rpy_ms.z > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) {
commanded_rudder = plane.guided_state.forced_rpy_cd.z;
} else {
commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
// add in rudder mixing from roll
commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix;
commanded_rudder += rudder_input;
}
steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500);
}
// move_yaw
void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out)
{
_yaw_servo.servo_out = constrain_int16(yaw_out, -4500, 4500);
if (_yaw_servo.servo_out != yaw_out) {
limit.yaw = true;
}
_yaw_servo.calc_pwm();
hal.rcout->write(AP_MOTORS_MOT_4, _yaw_servo.radio_out);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro);
} else {
write_aux(_ext_gyro_gain_std);
}
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) {
// output yaw servo to tail rsc
write_aux(_yaw_servo.servo_out);
}
}
/*
setup mixer on PX4 so that if FMU dies the pilot gets manual control
*/
bool Plane::setup_failsafe_mixing(void)
{
// we create MIXER.MIX regardless of whether we will be using it,
// as it gives a template for the user to modify to create their
// own CUSTOM.MIX file
const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX";
const char *custom_mixer_filename = "/fs/microsd/APM/CUSTOM.MIX";
bool ret = false;
char *buf = NULL;
const uint16_t buf_size = 2048;
if (!create_mixer_file(mixer_filename)) {
return false;
}
struct stat st;
const char *filename;
if (stat(custom_mixer_filename, &st) == 0) {
filename = custom_mixer_filename;
} else {
filename = mixer_filename;
}
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
int px4io_fd = open("/dev/px4io", 0);
if (px4io_fd == -1) {
// px4io isn't started, no point in setting up a mixer
return false;
}
buf = (char *)malloc(buf_size);
if (buf == NULL) {
goto failed;
}
if (load_mixer_file(filename, &buf[0], buf_size) != 0) {
hal.console->printf("Unable to load %s\n", filename);
goto failed;
}
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
// make sure the throttle has a non-zero failsafe value before we
// disable safety. This prevents sending zero PWM during switch over
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_min);
}
// we need to force safety on to allow us to load a mixer
hal.rcout->force_safety_on();
/* reset any existing mixer in px4io. This shouldn't be needed,
* but is good practice */
if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) {
hal.console->printf("Unable to reset mixer\n");
goto failed;
}
/* pass the buffer to the device */
if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) {
hal.console->printf("Unable to send mixer to IO\n");
goto failed;
}
// setup RC config for each channel based on user specified
// mix/max/trim. We only do the first 8 channels due to
// a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
for (uint8_t i=0; i<8; i++) {
RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == NULL) {
continue;
}
struct pwm_output_rc_config config;
/*
we use a min/max of 900/2100 to allow for pass-thru of
larger values than the RC min/max range. This mimics the APM
behaviour of pass-thru in manual, which allows for dual-rate
transmitter setups in manual mode to go beyond the ranges
used in stabilised modes
*/
config.channel = i;
config.rc_min = 900;
config.rc_max = 2100;
if (rcmap.throttle()-1 == i) {
// throttle uses a trim of 1500, so we don't get division
// by small numbers near RC3_MIN
config.rc_trim = 1500;
} else {
config.rc_trim = constrain_int16(ch->radio_trim, config.rc_min, config.rc_max);
}
config.rc_dz = 0; // zero for the purposes of manual takeover
config.rc_assignment = i;
// we set reverse as false, as users of ArduPilot will have
// input reversed on transmitter, so from the point of view of
// the mixer the input is never reversed. The one exception is
// the 2nd channel, which is reversed inside the PX4IO code,
// so needs to be unreversed here to give sane behaviour.
if (i == 1) {
config.rc_reverse = true;
} else {
config.rc_reverse = false;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) {
hal.console->printf("SET_RC_CONFIG failed\n");
goto failed;
}
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
pwm_values.values[i] = 900;
}
ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
pwm_values.values[i] = 2100;
}
ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0);
// setup for immediate manual control if FMU dies
ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1);
ret = true;
failed:
if (buf != NULL) {
free(buf);
}
if (px4io_fd != -1) {
close(px4io_fd);
}
// restore safety state if it was previously armed
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
hal.rcout->force_safety_off();
}
return ret;
}
/*
setup output channels all non-manual modes
*/
void Plane::set_servos_controlled(void)
{
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// allow landing to override servos if it would like to
landing.override_servos();
}
// convert 0 to 100% (or -100 to +100) into PWM
int8_t min_throttle = aparm.throttle_min.get();
int8_t max_throttle = aparm.throttle_max.get();
if (min_throttle < 0 && !allow_reverse_thrust()) {
// reverse thrust is available but inhibited.
min_throttle = 0;
}
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if(aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
}
} else if (landing.is_flaring()) {
min_throttle = 0;
}
// apply watt limiter
throttle_watt_limiter(min_throttle, max_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
if (!hal.util->get_soft_armed()) {
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE) &&
!failsafe.throttle_counter) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
} else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
} else if (quadplane.in_vtol_mode()) {
// ask quadplane code for forward throttle
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle));
}
// suppress throttle when soaring is active
if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE ||
control_mode == AUTO || control_mode == LOITER) &&
g2.soaring_controller.is_active() &&
g2.soaring_controller.get_throttle_suppressed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void Plane::set_servos(void)
{
int16_t last_throttle = channel_throttle->get_radio_out();
// do any transition updates for quadplane
quadplane.update();
if (control_mode == AUTO && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
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 (!RC_Channel_aux::function_assigned(RC_Channel_aux::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;
}
channel_rudder->set_servo_out(steering_control.rudder);
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
// do a direct pass through of radio values
if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
channel_roll->set_radio_out(channel_roll->get_radio_in());
channel_pitch->set_radio_out(channel_pitch->get_radio_in());
} else {
channel_roll->set_radio_out(channel_roll->read());
channel_pitch->set_radio_out(channel_pitch->read());
}
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
channel_rudder->set_radio_out(channel_rudder->get_radio_in());
// setup extra channels. We want this to come from the
// main input channel, but using the 2nd channels dead
// zone, reverse and min/max settings. We need to use
// pwm_to_angle_dz() to ensure we don't trim the value for the
// deadzone of the main aileron channel, otherwise the 2nd
// aileron won't quite follow the first one
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));
// this variant assumes you have the corresponding
// input channel setup in your transmitter for manual control
// of the 2nd aileron
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input);
if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) {
// set any differential spoilers to follow the elevons in
// manual mode.
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->get_radio_out());
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->get_radio_out());
}
} else {
if (g.mix_mode == 0) {
// both types of secondary aileron are slaved to the roll servo out
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = channel_pitch->get_servo_out() - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
ch2 = channel_pitch->get_servo_out() + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->get_servo_out());
/* Differential Spoilers
If differential spoilers are setup, then we translate
rudder control into splitting of the two ailerons on
the side of the aircraft where we want to induce
additional drag.
*/
if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
float ch3 = ch1;
float ch4 = ch2;
if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->get_servo_out() < 0) {
ch1 += abs(channel_rudder->get_servo_out());
ch3 -= abs(channel_rudder->get_servo_out());
} else {
ch2 += abs(channel_rudder->get_servo_out());
ch4 -= abs(channel_rudder->get_servo_out());
}
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler1, ch3);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_dspoiler2, ch4);
}
// directly set the radio_out values for elevon mode
channel_roll->set_radio_out(elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX)));
channel_pitch->set_radio_out(elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX)));
}
// push out the PWM values
if (g.mix_mode == 0) {
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
}
channel_rudder->calc_pwm();
#if THROTTLE_OUT == 0
channel_throttle->set_servo_out(0);
#else
// convert 0 to 100% (or -100 to +100) into PWM
int8_t min_throttle = aparm.throttle_min.get();
int8_t max_throttle = aparm.throttle_max.get();
if (min_throttle < 0 && !allow_reverse_thrust()) {
// reverse thrust is available but inhibited.
min_throttle = 0;
}
if (control_mode == AUTO) {
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
min_throttle = 0;
}
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
if(aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
}
}
}
uint32_t now = millis();
if (battery.overpower_detected()) {
// overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
// throttle limit will attack by 10% per second
if (channel_throttle->get_servo_out() > 0 && // demanding too much positive thrust
throttle_watt_limit_max < max_throttle - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
// always allow for 25% throttle available regardless of battery status
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max++;
} else if (channel_throttle->get_servo_out() < 0 &&
min_throttle < 0 && // reverse thrust is available
throttle_watt_limit_min < -(min_throttle) - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
// always allow for 25% throttle available regardless of battery status
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_min++;
}
} else if (now - throttle_watt_limit_timer_ms >= 1000) {
// it has been 1 second since last over-current, check if we can resume higher throttle.
// this throttle release is needed to allow raising the max_throttle as the battery voltage drains down
// throttle limit will release by 1% per second
if (channel_throttle->get_servo_out() > throttle_watt_limit_max && // demanding max forward thrust
throttle_watt_limit_max > 0) { // and we're currently limiting it
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max--;
} else if (channel_throttle->get_servo_out() < throttle_watt_limit_min && // demanding max negative thrust
throttle_watt_limit_min > 0) { // and we're limiting it
throttle_watt_limit_timer_ms = now;
throttle_watt_limit_min--;
}
}
max_throttle = constrain_int16(max_throttle, 0, max_throttle - throttle_watt_limit_max);
if (min_throttle < 0) {
min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);
}
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
min_throttle,
max_throttle));
if (!hal.util->get_soft_armed()) {
channel_throttle->set_servo_out(0);
channel_throttle->calc_pwm();
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
channel_throttle->set_servo_out(0);
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else {
channel_throttle->calc_pwm();
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE) &&
!failsafe.ch3_counter) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else if (control_mode == GUIDED &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else if (quadplane.in_vtol_mode()) {
// ask quadplane code for forward throttle
channel_throttle->set_servo_out(quadplane.forward_throttle_pct());
channel_throttle->calc_pwm();
} else {
// normal throttle calculation based on servo_out
channel_throttle->calc_pwm();
}
#endif
}
// Auto flap deployment
int8_t auto_flap_percent = 0;
int8_t manual_flap_percent = 0;
static int8_t last_auto_flap;
static int8_t last_manual_flap;
// work out any manual flap input
RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
flapin->input();
manual_flap_percent = flapin->percent_input();
}
if (auto_throttle_mode) {
int16_t flapSpeedSource = 0;
if (ahrs.airspeed_sensor_enabled()) {
flapSpeedSource = target_airspeed_cm * 0.01f;
} else {
flapSpeedSource = aparm.throttle_cruise;
}
if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
auto_flap_percent = g.flap_2_percent;
} else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
/*
special flap levels for takeoff and landing. This works
better than speed based flaps as it leads to less
possibility of oscillation
*/
if (control_mode == AUTO) {
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
if (g.takeoff_flap_percent != 0) {
auto_flap_percent = g.takeoff_flap_percent;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
if (g.land_flap_percent != 0) {
auto_flap_percent = g.land_flap_percent;
}
break;
default:
break;
}
}
}
// manual flap input overrides auto flap input
if (abs(manual_flap_percent) > auto_flap_percent) {
auto_flap_percent = manual_flap_percent;
}
flap_slew_limit(last_auto_flap, auto_flap_percent);
flap_slew_limit(last_manual_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent);
if (control_mode >= FLY_BY_WIRE_B ||
quadplane.in_assisted_flight() ||
quadplane.in_vtol_mode()) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit(last_throttle);
}
if (control_mode == TRAINING) {
// copy rudder in training mode
channel_rudder->set_radio_out(channel_rudder->get_radio_in());
}
if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
flaperon_update(auto_flap_percent);
}
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch, channel_rudder);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch, channel_roll);
}
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:
channel_throttle->set_radio_out(0);
break;
case AP_Arming::YES_MIN_PWM:
default:
channel_throttle->set_radio_out(throttle_min());
break;
}
}
#if OBC_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
obc.check_crash_plane();
#endif
#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) {
return;
}
}
#endif
if (g.land_then_servos_neutral > 0 &&
control_mode == AUTO &&
g.land_disarm_delay > 0 &&
auto_state.land_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 (g.land_then_servos_neutral == 1) {
channel_roll->set_radio_out(channel_roll->get_radio_trim());
channel_pitch->set_radio_out(channel_pitch->get_radio_trim());
channel_rudder->set_radio_out(channel_rudder->get_radio_trim());
} else if (g.land_then_servos_neutral == 2) {
channel_roll->disable_out();
channel_pitch->disable_out();
channel_rudder->disable_out();
}
}
// send values to the PWM timers for output
// ----------------------------------------
if (g.rudder_only == 0) {
// when we RUDDER_ONLY mode we don't send the channel_roll
// output and instead rely on KFF_RDDRMIX. That allows the yaw
// damper to operate.
channel_roll->output();
}
channel_pitch->output();
channel_throttle->output();
channel_rudder->output();
RC_Channel_aux::output_ch_all();
}
void AP_LandingGear::update(float height_above_ground_m)
{
if (_pin_weight_on_wheels == -1) {
last_wow_event_ms = 0;
wow_state_current = LG_WOW_UNKNOWN;
} else {
LG_WOW_State wow_state_new = hal.gpio->read(_pin_weight_on_wheels) == _pin_weight_on_wheels_polarity ? LG_WOW : LG_NO_WOW;
if (wow_state_new != wow_state_current) {
// we changed states, lets note the time.
last_wow_event_ms = AP_HAL::millis();
log_wow_state(wow_state_new);
}
wow_state_current = wow_state_new;
}
if (_pin_deployed == -1) {
last_gear_event_ms = 0;
// If there was no pilot input and state is still unknown - leave it as it is
if (gear_state_current != LG_UNKNOWN) {
gear_state_current = (_deployed == true ? LG_DEPLOYED : LG_RETRACTED);
}
} else {
LG_LandingGear_State gear_state_new;
if (_deployed) {
gear_state_new = (deployed() == true ? LG_DEPLOYED : LG_DEPLOYING);
} else {
gear_state_new = (deployed() == false ? LG_RETRACTED : LG_RETRACTING);
}
if (gear_state_new != gear_state_current) {
// we changed states, lets note the time.
last_gear_event_ms = AP_HAL::millis();
log_wow_state(wow_state_current);
}
gear_state_current = gear_state_new;
}
/*
check for height based triggering
*/
int16_t alt_m = constrain_int16(height_above_ground_m, 0, INT16_MAX);
if (hal.util->get_soft_armed()) {
// only do height based triggering when armed
if ((!_deployed || !_have_changed) &&
_deploy_alt > 0 &&
alt_m <= _deploy_alt &&
_last_height_above_ground > _deploy_alt) {
deploy();
}
if ((_deployed || !_have_changed) &&
_retract_alt > 0 &&
_retract_alt >= _deploy_alt &&
alt_m >= _retract_alt &&
_last_height_above_ground < _retract_alt) {
retract();
}
}
_last_height_above_ground = alt_m;
}
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void AP_MotorsHeli::set_delta_phase_angle(int16_t angle)
{
angle = constrain_int16(angle, -90, 90);
_delta_phase_angle = angle;
calculate_roll_pitch_collective_factors();
}
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
{
int16_t channels[11];
float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
int16_t rpyCenter = 1500;
int16_t throttleBase = 1500-500*throttleScale;
bool shift = false;
// Neutralize camera tilt speed setpoint
cam_tilt = 1500;
// Detect if any shift button is pressed
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
shift = true;
}
}
// Act if button is pressed
// Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i))) {
handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
}
}
buttons_prev = buttons;
// Set channels to override
if (!roll_pitch_flag) {
channels[0] = 1500 + pitchTrim; // pitch
channels[1] = 1500 + rollTrim; // roll
} else {
// adjust roll and pitch with joystick input instead of forward and lateral
channels[0] = constrain_int16((x+pitchTrim)*rpyScale+rpyCenter,1100,1900);
channels[1] = constrain_int16((y+rollTrim)*rpyScale+rpyCenter,1100,1900);
}
channels[2] = constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900); // throttle
channels[3] = constrain_int16(r*rpyScale+rpyCenter,1100,1900); // yaw
if (!roll_pitch_flag) {
// adjust forward and lateral with joystick input instead of roll and pitch
channels[4] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900); // forward for ROV
channels[5] = constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900); // lateral for ROV
} else {
// neutralize forward and lateral input while we are adjusting roll and pitch
channels[4] = constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900); // forward for ROV
channels[5] = constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900); // lateral for ROV
}
channels[6] = 0; // Unused
channels[7] = cam_tilt; // camera tilt
channels[8] = lights1; // lights 1
channels[9] = lights2; // lights 2
channels[10] = video_switch; // video switch
// Store old x, y, z values for use in input hold logic
x_last = x;
y_last = y;
z_last = z;
hal.rcin->set_overrides(channels, 10);
}
/*
setup output channels all non-manual modes
*/
void Plane::set_servos_controlled(void)
{
if (g.mix_mode != 0) {
set_servos_old_elevons();
} else {
// both types of secondary aileron are slaved to the roll servo out
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron, channel_roll->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_aileron_with_input, channel_roll->get_servo_out());
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator, channel_pitch->get_servo_out());
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_elevator_with_input, channel_pitch->get_servo_out());
// push out the PWM values
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
}
channel_rudder->calc_pwm();
// convert 0 to 100% (or -100 to +100) into PWM
int8_t min_throttle = aparm.throttle_min.get();
int8_t max_throttle = aparm.throttle_max.get();
if (min_throttle < 0 && !allow_reverse_thrust()) {
// reverse thrust is available but inhibited.
min_throttle = 0;
}
if (control_mode == AUTO) {
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
min_throttle = 0;
}
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
if(aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
}
}
}
// apply watt limiter
throttle_watt_limiter(min_throttle, max_throttle);
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
min_throttle,
max_throttle));
if (!hal.util->get_soft_armed()) {
channel_throttle->set_servo_out(0);
channel_throttle->calc_pwm();
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
channel_throttle->set_servo_out(0);
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else {
channel_throttle->calc_pwm();
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE) &&
!failsafe.ch3_counter) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->set_radio_out(channel_throttle->get_radio_in());
} else if (quadplane.in_vtol_mode()) {
// ask quadplane code for forward throttle
channel_throttle->set_servo_out(quadplane.forward_throttle_pct());
channel_throttle->calc_pwm();
} else {
// normal throttle calculation based on servo_out
channel_throttle->calc_pwm();
}
}
/*
implement a software VTail or elevon mixer. There are 4 different mixing modes
*/
void Plane::channel_output_mixer(uint8_t mixing_type, int16_t & chan1_out, int16_t & chan2_out)const
{
int16_t c1, c2;
int16_t v1, v2;
// first get desired elevator and rudder as -500..500 values
c1 = chan1_out - 1500;
c2 = chan2_out - 1500;
// apply MIXING_OFFSET to input channels using long-integer version
// of formula: x = x * (g.mixing_offset/100.0 + 1.0)
// -100 => 2x on 'c1', 100 => 2x on 'c2'
if (g.mixing_offset < 0) {
c1 = (int16_t)(((int32_t)c1) * (-g.mixing_offset+100) / 100);
} else if (g.mixing_offset > 0) {
c2 = (int16_t)(((int32_t)c2) * (g.mixing_offset+100) / 100);
}
v1 = (c1 - c2) * g.mixing_gain;
v2 = (c1 + c2) * g.mixing_gain;
// now map to mixed output
switch (mixing_type) {
case MIXING_DISABLED:
return;
case MIXING_UPUP:
break;
case MIXING_UPDN:
v2 = -v2;
break;
case MIXING_DNUP:
v1 = -v1;
break;
case MIXING_DNDN:
v1 = -v1;
v2 = -v2;
break;
case MIXING_UPUP_SWP:
std::swap(v1, v2);
break;
case MIXING_UPDN_SWP:
v2 = -v2;
std::swap(v1, v2);
break;
case MIXING_DNUP_SWP:
v1 = -v1;
std::swap(v1, v2);
break;
case MIXING_DNDN_SWP:
v1 = -v1;
v2 = -v2;
std::swap(v1, v2);
break;
}
// scale for a 1500 center and 900..2100 range, symmetric
v1 = constrain_int16(v1, -600, 600);
v2 = constrain_int16(v2, -600, 600);
chan1_out = 1500 + v1;
chan2_out = 1500 + v2;
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void Plane::set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
if (control_mode == AUTO && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
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 (!RC_Channel_aux::function_assigned(RC_Channel_aux::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;
}
channel_rudder->servo_out = steering_control.rudder;
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
// do a direct pass through of radio values
if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
channel_roll->radio_out = channel_roll->radio_in;
channel_pitch->radio_out = channel_pitch->radio_in;
} else {
channel_roll->radio_out = channel_roll->read();
channel_pitch->radio_out = channel_pitch->read();
}
channel_throttle->radio_out = channel_throttle->radio_in;
channel_rudder->radio_out = channel_rudder->radio_in;
// setup extra channels. We want this to come from the
// main input channel, but using the 2nd channels dead
// zone, reverse and min/max settings. We need to use
// pwm_to_angle_dz() to ensure we don't trim the value for the
// deadzone of the main aileron channel, otherwise the 2nd
// aileron won't quite follow the first one
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));
// this variant assumes you have the corresponding
// input channel setup in your transmitter for manual control
// of the 2nd aileron
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input);
if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) {
// set any differential spoilers to follow the elevons in
// manual mode.
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out);
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out);
}
} else {
if (g.mix_mode == 0) {
// both types of secondary aileron are slaved to the roll servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out);
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out);
} else {
/*Elevon mode*/
float ch1;
float ch2;
ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
/* Differential Spoilers
If differential spoilers are setup, then we translate
rudder control into splitting of the two ailerons on
the side of the aircraft where we want to induce
additional drag.
*/
if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
float ch3 = ch1;
float ch4 = ch2;
if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) {
ch1 += abs(channel_rudder->servo_out);
ch3 -= abs(channel_rudder->servo_out);
} else {
ch2 += abs(channel_rudder->servo_out);
ch4 -= abs(channel_rudder->servo_out);
}
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4);
}
// directly set the radio_out values for elevon mode
channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX));
channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX));
}
// push out the PWM values
if (g.mix_mode == 0) {
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
}
channel_rudder->calc_pwm();
#if THROTTLE_OUT == 0
channel_throttle->servo_out = 0;
#else
// convert 0 to 100% into PWM
uint8_t min_throttle = aparm.throttle_min.get();
uint8_t max_throttle = aparm.throttle_max.get();
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
min_throttle = 0;
}
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
if(aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
}
}
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
min_throttle,
max_throttle);
if (!hal.util->get_soft_armed()) {
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
channel_throttle->servo_out = 0;
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
channel_throttle->radio_out = channel_throttle->radio_in;
} else {
channel_throttle->calc_pwm();
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE)) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->radio_out = channel_throttle->radio_in;
} else if (control_mode == GUIDED &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
} else {
// normal throttle calculation based on servo_out
channel_throttle->calc_pwm();
}
#endif
}
// Auto flap deployment
int8_t auto_flap_percent = 0;
int8_t manual_flap_percent = 0;
static int8_t last_auto_flap;
static int8_t last_manual_flap;
// work out any manual flap input
RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
flapin->input();
manual_flap_percent = flapin->percent_input();
}
if (auto_throttle_mode) {
int16_t flapSpeedSource = 0;
if (ahrs.airspeed_sensor_enabled()) {
flapSpeedSource = target_airspeed_cm * 0.01f;
} else {
flapSpeedSource = aparm.throttle_cruise;
}
if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
auto_flap_percent = g.flap_2_percent;
} else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
/*
special flap levels for takeoff and landing. This works
better than speed based flaps as it leads to less
possibility of oscillation
*/
if (control_mode == AUTO) {
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
if (g.takeoff_flap_percent != 0) {
auto_flap_percent = g.takeoff_flap_percent;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
if (g.land_flap_percent != 0) {
auto_flap_percent = g.land_flap_percent;
}
break;
default:
break;
}
}
}
// manual flap input overrides auto flap input
if (abs(manual_flap_percent) > auto_flap_percent) {
auto_flap_percent = manual_flap_percent;
}
flap_slew_limit(last_auto_flap, auto_flap_percent);
flap_slew_limit(last_manual_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent);
if (control_mode >= FLY_BY_WIRE_B) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit(last_throttle);
}
if (control_mode == TRAINING) {
// copy rudder in training mode
channel_rudder->radio_out = channel_rudder->radio_in;
}
if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
flaperon_update(auto_flap_percent);
}
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
}
//send throttle to 0 or MIN_PWM if not yet armed
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::YES_MIN_PWM:
channel_throttle->radio_out = channel_throttle->radio_min;
break;
case AP_Arming::YES_ZERO_PWM:
channel_throttle->radio_out = 0;
break;
default:
//keep existing behavior: do nothing to radio_out
//(don't disarm throttle channel even if AP_Arming class is)
break;
}
}
#if OBC_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
obc.check_crash_plane();
#endif
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// get the servos to the GCS immediately for HIL
if (comm_get_txspace(MAVLINK_COMM_0) >=
MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
}
}
#endif
// send values to the PWM timers for output
// ----------------------------------------
if (g.rudder_only == 0) {
// when we RUDDER_ONLY mode we don't send the channel_roll
// output and instead rely on KFF_RDDRMIX. That allows the yaw
// damper to operate.
channel_roll->output();
}
channel_pitch->output();
channel_throttle->output();
channel_rudder->output();
RC_Channel_aux::output_ch_all();
}
// sanity check parameters
void AP_MotorsUGV::sanity_check_parameters()
{
_throttle_min = constrain_int16(_throttle_min, 0, 20);
_throttle_max = constrain_int16(_throttle_max, 30, 100);
_vector_throttle_base = constrain_float(_vector_throttle_base, 0.0f, 100.0f);
}
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
{
float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
int16_t rpyCenter = 1500;
int16_t throttleBase = 1500-500*throttleScale;
bool shift = false;
// Neutralize camera tilt and pan speed setpoint
cam_tilt = 1500;
cam_pan = 1500;
// Detect if any shift button is pressed
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
shift = true;
}
}
// Act if button is pressed
// Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
for (uint8_t i = 0 ; i < 16 ; i++) {
if ((buttons & (1 << i))) {
handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
// buttonDebounce = tnow_ms;
} else if (buttons_prev & (1 << i)) {
handle_jsbutton_release(i, shift);
}
}
buttons_prev = buttons;
// attitude mode:
if (roll_pitch_flag == 1) {
// adjust roll/pitch trim with joystick input instead of forward/lateral
pitchTrim = -x * rpyScale;
rollTrim = y * rpyScale;
}
uint32_t tnow = AP_HAL::millis();
RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch
RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900), tnow); // roll
RC_Channels::set_override(2, constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900), tnow); // throttle
RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow); // yaw
// maneuver mode:
if (roll_pitch_flag == 0) {
// adjust forward and lateral with joystick input instead of roll and pitch
RC_Channels::set_override(4, constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
RC_Channels::set_override(5, constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
} else {
// neutralize forward and lateral input while we are adjusting roll and pitch
RC_Channels::set_override(4, constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
}
RC_Channels::set_override(6, cam_pan, tnow); // camera pan
RC_Channels::set_override(7, cam_tilt, tnow); // camera tilt
RC_Channels::set_override(8, lights1, tnow); // lights 1
RC_Channels::set_override(9, lights2, tnow); // lights 2
RC_Channels::set_override(10, video_switch, tnow); // video switch
// Store old x, y, z values for use in input hold logic
x_last = x;
y_last = y;
z_last = z;
}
// output_armed - sends commands to the motors
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max;
int16_t rc_yaw_constrained_pwm;
int16_t rc_yaw_excess;
int16_t upper_margin, lower_margin;
int16_t motor_adjustment = 0;
int16_t yaw_to_execute = 0;
// initialize reached_limit flag
_reached_limit = AP_MOTOR_NO_LIMITS_REACHED;
// Throttle is 0 to 1000 only
_rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
}
}
// if we have any roll, pitch or yaw input then it's breaching the limit
if( _rc_roll->pwm_out != 0 || _rc_pitch->pwm_out != 0 ) {
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT;
}
if( _rc_yaw->pwm_out != 0 ) {
_reached_limit |= AP_MOTOR_YAW_LIMIT;
}
} else { // non-zero throttle
out_min = _rc_throttle->radio_min + _min_throttle;
// initialise rc_yaw_contrained_pwm that we will certainly output and rc_yaw_excess that we will do on best-efforts basis.
// Note: these calculations and many others below depend upon _yaw_factors always being 0, -1 or 1.
if( _rc_yaw->pwm_out < -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) {
rc_yaw_constrained_pwm = -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
rc_yaw_excess = _rc_yaw->pwm_out+AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
}else if( _rc_yaw->pwm_out > AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) {
rc_yaw_constrained_pwm = AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
rc_yaw_excess = _rc_yaw->pwm_out-AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM;
}else{
rc_yaw_constrained_pwm = _rc_yaw->pwm_out;
rc_yaw_excess = 0;
}
// initialise upper and lower margins
upper_margin = lower_margin = out_max - out_min;
// add roll, pitch, throttle and constrained yaw for each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_out +
_rc_roll->pwm_out * _roll_factor[i] +
_rc_pitch->pwm_out * _pitch_factor[i] +
rc_yaw_constrained_pwm * _yaw_factor[i];
// calculate remaining room between fastest running motor and top of pwm range
if( out_max - motor_out[i] < upper_margin) {
upper_margin = out_max - motor_out[i];
}
// calculate remaining room between slowest running motor and bottom of pwm range
if( motor_out[i] - out_min < lower_margin ) {
lower_margin = motor_out[i] - out_min;
}
}
}
// if motors are running too fast and we have enough room below, lower overall throttle
if( upper_margin < 0 || lower_margin < 0 ) {
// calculate throttle adjustment that equalizes upper and lower margins. We will never push the throttle beyond this point
motor_adjustment = (upper_margin - lower_margin) / 2; // i.e. if overflowed by 20 on top, 30 on bottom, upper_margin = -20, lower_margin = -30. will adjust motors -5.
// if we have overflowed on the top, reduce but no more than to the mid point
if( upper_margin < 0 ) {
motor_adjustment = max(upper_margin, motor_adjustment);
}
// if we have underflowed on the bottom, increase throttle but no more than to the mid point
if( lower_margin < 0 ) {
motor_adjustment = min(-lower_margin, motor_adjustment);
}
}
// move throttle up or down to to pull within tolerance
if( motor_adjustment != 0 ) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] += motor_adjustment;
}
}
// we haven't even been able to apply roll, pitch and minimal yaw without adjusting throttle so mark all limits as breached
_reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT | AP_MOTOR_YAW_LIMIT | AP_MOTOR_THROTTLE_LIMIT;
}
// if we didn't give all the yaw requested, calculate how much additional yaw we can add
if( rc_yaw_excess != 0 ) {
// try for everything
yaw_to_execute = rc_yaw_excess;
// loop through motors and reduce as necessary
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && _yaw_factor[i] != 0 ) {
// calculate upper and lower margins for this motor
upper_margin = max(0,out_max - motor_out[i]);
lower_margin = max(0,motor_out[i] - out_min);
// motor is increasing, check upper limit
if( rc_yaw_excess > 0 && _yaw_factor[i] > 0 ) {
yaw_to_execute = min(yaw_to_execute, upper_margin);
}
// motor is decreasing, check lower limit
if( rc_yaw_excess > 0 && _yaw_factor[i] < 0 ) {
yaw_to_execute = min(yaw_to_execute, lower_margin);
}
// motor is decreasing, check lower limit
if( rc_yaw_excess < 0 && _yaw_factor[i] > 0 ) {
yaw_to_execute = max(yaw_to_execute, -lower_margin);
}
// motor is increasing, check upper limit
if( rc_yaw_excess < 0 && _yaw_factor[i] < 0 ) {
yaw_to_execute = max(yaw_to_execute, -upper_margin);
}
}
}
// check yaw_to_execute is reasonable
if( yaw_to_execute != 0 && ((yaw_to_execute>0 && rc_yaw_excess>0) || (yaw_to_execute<0 && rc_yaw_excess<0)) ) {
// add the additional yaw
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] += _yaw_factor[i] * yaw_to_execute;
}
}
}
// mark yaw limit reached if we didn't get everything we asked for
if( yaw_to_execute != rc_yaw_excess ) {
_reached_limit |= AP_MOTOR_YAW_LIMIT;
}
}
// adjust for throttle curve
if( _throttle_curve_enabled ) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _throttle_curve.get_y(motor_out[i]);
}
}
}
// clip motor output if required (shouldn't be)
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = constrain_int16(motor_out[i], out_min, out_max);
}
}
}
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->write(_motor_to_channel_map[i], motor_out[i]);
}
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
void AP_MotorsMatrix::output_armed_stabilizing()
{
int8_t i;
int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits
int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors
int16_t rpy_low = 0; // lowest motor value
int16_t rpy_high = 0; // highest motor value
int16_t yaw_allowed; // amount of yaw we can fit in
int16_t thr_adj; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Ensure throttle is within bounds of 0 to 1000
int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle);
if (_throttle_control_input <= thr_in_min) {
_throttle_control_input = thr_in_min;
limit.throttle_lower = true;
}
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
roll_pwm = calc_roll_pwm();
pitch_pwm = calc_pitch_pwm();
yaw_pwm = calc_yaw_pwm();
throttle_radio_output = calc_throttle_radio_output();
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = roll_pwm * _roll_factor[i] * get_compensation_gain() +
pitch_pwm * _pitch_factor[i] * get_compensation_gain()+yaw_pwm*_yaw_factor[i]*(1000-m_Conv)/1000;
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
rpy_low = rpy_out[i];
}
// record highest roll pich command
if (rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. mid throttle - average of highest and lowest motor (this would give the maximum possible room margin above the highest motor and below the lowest)
// 2. the higher of:
// a) the pilot's throttle input
// b) the mid point between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it
int16_t motor_mid = (rpy_low+rpy_high)/2;
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(throttle_radio_output, throttle_radio_output*max(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix));
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2;
yaw_allowed = max(yaw_allowed, _yaw_headroom);
if (yaw_pwm >= 0) {
// if yawing right
if (yaw_allowed > yaw_pwm * get_compensation_gain()) {
yaw_allowed = yaw_pwm * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}else{
// if yawing left
yaw_allowed = -yaw_allowed;
if (yaw_allowed < yaw_pwm * get_compensation_gain()) {
yaw_allowed = yaw_pwm * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}
// add yaw to intermediate numbers for each motor
rpy_low = 0;
rpy_high = 0;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
//rpy_out[i] = rpy_out[i] +
// yaw_allowed * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if( rpy_out[i] < rpy_low ) {
rpy_low = rpy_out[i];
}
// record highest roll+pitch+yaw command
if( rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// check everything fits
thr_adj = throttle_radio_output - out_best_thr_pwm;
// calculate upper and lower limits of thr_adj
int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0);
// if we are increasing the throttle (situation #2 above)..
if (thr_adj > 0) {
// increase throttle as close as possible to requested throttle
// without going over out_max_pwm
if (thr_adj > thr_adj_max){
thr_adj = thr_adj_max;
// we haven't even been able to apply full throttle command
limit.throttle_upper = true;
}
}else if(thr_adj < 0){
// decrease throttle as close as possible to requested throttle
// without going under out_min_pwm or over out_max_pwm
// earlier code ensures we can't break both boundaries
int16_t thr_adj_min = min(out_min_pwm-(out_best_thr_pwm+rpy_low),0);
if (thr_adj > thr_adj_max) {
thr_adj = thr_adj_max;
limit.throttle_upper = true;
}
if (thr_adj < thr_adj_min) {
thr_adj = thr_adj_min;
}
}
// do we need to reduce roll, pitch, yaw command
// earlier code does not allow both limit's to be passed simultaneously with abs(_yaw_factor)<1
if ((rpy_low+out_best_thr_pwm)+thr_adj < out_min_pwm){
// protect against divide by zero
if (rpy_low != 0) {
rpy_scale = (float)(out_min_pwm-thr_adj-out_best_thr_pwm)/rpy_low;
}
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}else if((rpy_high+out_best_thr_pwm)+thr_adj > out_max_pwm){
// protect against divide by zero
if (rpy_high != 0) {
rpy_scale = (float)(out_max_pwm-thr_adj-out_best_thr_pwm)/rpy_high;
}
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = out_best_thr_pwm+thr_adj +
rpy_scale*rpy_out[i];
}
}
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), motor_out[i]);
}
}
}
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
{
// Act based on the function assigned to this button
switch (get_button(button)->function(shift)) {
case JSButton::button_function_t::k_arm_toggle:
if (motors.armed()) {
init_disarm_motors();
} else {
init_arm_motors(true);
}
break;
case JSButton::button_function_t::k_arm:
init_arm_motors(true);
break;
case JSButton::button_function_t::k_disarm:
init_disarm_motors();
break;
case JSButton::button_function_t::k_mode_manual:
set_mode(MANUAL, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_stabilize:
set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_depth_hold:
set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_auto:
set_mode(AUTO, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_guided:
set_mode(GUIDED, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_circle:
set_mode(CIRCLE, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_acro:
set_mode(ACRO, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_poshold:
set_mode(POSHOLD, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mount_center:
camera_mount.set_angle_targets(0, 0, 0);
// for some reason the call to set_angle_targets changes the mode to mavlink targeting!
camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
break;
case JSButton::button_function_t::k_mount_tilt_up:
cam_tilt = 1900;
break;
case JSButton::button_function_t::k_mount_tilt_down:
cam_tilt = 1100;
break;
case JSButton::button_function_t::k_camera_trigger:
break;
case JSButton::button_function_t::k_camera_source_toggle:
if (!held) {
static bool video_toggle = false;
video_toggle = !video_toggle;
if (video_toggle) {
video_switch = 1900;
gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2");
} else {
video_switch = 1100;
gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1");
}
}
break;
case JSButton::button_function_t::k_mount_pan_right:
// Not implemented
break;
case JSButton::button_function_t::k_mount_pan_left:
// Not implemented
break;
case JSButton::button_function_t::k_lights1_cycle:
if (!held) {
static bool increasing = true;
RC_Channel* chan = RC_Channels::rc_channel(8);
uint16_t min = chan->get_radio_min();
uint16_t max = chan->get_radio_max();
uint16_t step = (max - min) / g.lights_steps;
if (increasing) {
lights1 = constrain_float(lights1 + step, min, max);
} else {
lights1 = constrain_float(lights1 - step, min, max);
}
if (lights1 >= max || lights1 <= min) {
increasing = !increasing;
}
}
break;
case JSButton::button_function_t::k_lights1_brighter:
if (!held) {
RC_Channel* chan = RC_Channels::rc_channel(8);
uint16_t min = chan->get_radio_min();
uint16_t max = chan->get_radio_max();
uint16_t step = (max - min) / g.lights_steps;
lights1 = constrain_float(lights1 + step, min, max);
}
break;
case JSButton::button_function_t::k_lights1_dimmer:
if (!held) {
RC_Channel* chan = RC_Channels::rc_channel(8);
uint16_t min = chan->get_radio_min();
uint16_t max = chan->get_radio_max();
uint16_t step = (max - min) / g.lights_steps;
lights1 = constrain_float(lights1 - step, min, max);
}
break;
case JSButton::button_function_t::k_lights2_cycle:
if (!held) {
static bool increasing = true;
RC_Channel* chan = RC_Channels::rc_channel(9);
uint16_t min = chan->get_radio_min();
uint16_t max = chan->get_radio_max();
uint16_t step = (max - min) / g.lights_steps;
if (increasing) {
lights2 = constrain_float(lights2 + step, min, max);
} else {
lights2 = constrain_float(lights2 - step, min, max);
}
if (lights2 >= max || lights2 <= min) {
increasing = !increasing;
}
}
break;
case JSButton::button_function_t::k_lights2_brighter:
if (!held) {
RC_Channel* chan = RC_Channels::rc_channel(9);
uint16_t min = chan->get_radio_min();
uint16_t max = chan->get_radio_max();
uint16_t step = (max - min) / g.lights_steps;
lights2 = constrain_float(lights2 + step, min, max);
}
break;
case JSButton::button_function_t::k_lights2_dimmer:
if (!held) {
RC_Channel* chan = RC_Channels::rc_channel(9);
uint16_t min = chan->get_radio_min();
uint16_t max = chan->get_radio_max();
uint16_t step = (max - min) / g.lights_steps;
lights2 = constrain_float(lights2 - step, min, max);
}
break;
case JSButton::button_function_t::k_gain_toggle:
if (!held) {
static bool lowGain = false;
lowGain = !lowGain;
if (lowGain) {
gain = 0.5f;
} else {
gain = 1.0f;
}
gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100);
}
break;
case JSButton::button_function_t::k_gain_inc:
if (!held) {
// check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
if (g.numGainSettings == 1) {
gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
} else {
gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
}
gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
}
break;
case JSButton::button_function_t::k_gain_dec:
if (!held) {
// check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
if (g.numGainSettings == 1) {
gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
} else {
gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
}
gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
}
break;
case JSButton::button_function_t::k_trim_roll_inc:
rollTrim = constrain_float(rollTrim+10,-200,200);
break;
case JSButton::button_function_t::k_trim_roll_dec:
rollTrim = constrain_float(rollTrim-10,-200,200);
break;
case JSButton::button_function_t::k_trim_pitch_inc:
pitchTrim = constrain_float(pitchTrim+10,-200,200);
break;
case JSButton::button_function_t::k_trim_pitch_dec:
pitchTrim = constrain_float(pitchTrim-10,-200,200);
break;
case JSButton::button_function_t::k_input_hold_set:
if(!motors.armed()) {
break;
}
if (!held) {
zTrim = abs(z_last-500) > 50 ? z_last-500 : 0;
xTrim = abs(x_last) > 50 ? x_last : 0;
yTrim = abs(y_last) > 50 ? y_last : 0;
bool input_hold_engaged_last = input_hold_engaged;
input_hold_engaged = zTrim || xTrim || yTrim;
if (input_hold_engaged) {
gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
} else if (input_hold_engaged_last) {
gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Disabled");
}
}
break;
case JSButton::button_function_t::k_relay_1_on:
relay.on(0);
break;
case JSButton::button_function_t::k_relay_1_off:
relay.off(0);
break;
case JSButton::button_function_t::k_relay_1_toggle:
if (!held) {
relay.toggle(0);
}
break;
case JSButton::button_function_t::k_relay_2_on:
relay.on(1);
break;
case JSButton::button_function_t::k_relay_2_off:
relay.off(1);
break;
case JSButton::button_function_t::k_relay_2_toggle:
if (!held) {
relay.toggle(1);
}
break;
case JSButton::button_function_t::k_relay_3_on:
relay.on(2);
break;
case JSButton::button_function_t::k_relay_3_off:
relay.off(2);
break;
case JSButton::button_function_t::k_relay_3_toggle:
if (!held) {
relay.toggle(2);
}
break;
case JSButton::button_function_t::k_relay_4_on:
relay.on(3);
break;
case JSButton::button_function_t::k_relay_4_off:
relay.off(3);
break;
case JSButton::button_function_t::k_relay_4_toggle:
if (!held) {
relay.toggle(3);
}
break;
////////////////////////////////////////////////
// Servo functions
// TODO: initialize
case JSButton::button_function_t::k_servo_1_inc:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed
pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_1_dec:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed
pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_1_min:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_1_max:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_1_center:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_2_inc:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed
pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_2_dec:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed
pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_2_min:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_2_max:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_2_center:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_3_inc:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed
pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_3_dec:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed
pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_3_min:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_3_max:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed
}
break;
case JSButton::button_function_t::k_servo_3_center:
{
SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed
}
break;
case JSButton::button_function_t::k_roll_pitch_toggle:
if (!held) {
roll_pitch_flag = !roll_pitch_flag;
}
break;
case JSButton::button_function_t::k_custom_1:
// Not implemented
break;
case JSButton::button_function_t::k_custom_2:
// Not implemented
break;
case JSButton::button_function_t::k_custom_3:
// Not implemented
break;
case JSButton::button_function_t::k_custom_4:
// Not implemented
break;
case JSButton::button_function_t::k_custom_5:
// Not implemented
break;
case JSButton::button_function_t::k_custom_6:
// Not implemented
break;
}
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// yaw: -4500 ~ 4500
//
void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
{
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
if( servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to free up the swash
if( _swash_initialised ) {
reset_swash();
}
coll_out_scaled = coll_in * _collective_scalar + _rc_throttle->radio_min - 1000;
}else{ // regular flight mode
// check if we need to reinitialise the swash
if( !_swash_initialised ) {
init_swash();
}
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// coming into this equation at 4500 or less, and based on the original assumption of the
// total _servo_x.servo_out range being -4500 to 4500.
roll_out = roll_out * _roll_scaler;
roll_out = constrain_int16(roll_out, (int16_t)-roll_max, (int16_t)roll_max);
pitch_out = pitch_out * _pitch_scaler;
pitch_out = constrain_int16(pitch_out, (int16_t)-pitch_max, (int16_t)pitch_max);
// scale collective pitch
coll_out = constrain_int16(coll_in, 0, 1000);
if (stab_throttle){
coll_out = coll_out * _stab_throttle_scalar + stab_col_min*10;
}
coll_out_scaled = coll_out * _collective_scalar + collective_min - 1000;
// rudder feed forward based on collective
if( !ext_gyro_enabled ) {
yaw_offset = collective_yaw_effect * abs(coll_out_scaled - throttle_mid);
}
}
// swashplate servos
_servo_1->servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1->radio_trim-1500);
_servo_2->servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2->radio_trim-1500);
if( swash_type == AP_MOTORS_HELI_SWASH_H1 ) {
_servo_1->servo_out += 500;
_servo_2->servo_out += 500;
}
_servo_3->servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3->radio_trim-1500);
_servo_4->servo_out = yaw_out + yaw_offset;
// use servo_out to calculate pwm_out and radio_out
_servo_1->calc_pwm();
_servo_2->calc_pwm();
_servo_3->calc_pwm();
_servo_4->calc_pwm();
// actually move the servos
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], _servo_1->radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_2], _servo_2->radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_3], _servo_3->radio_out);
hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_4], _servo_4->radio_out);
// to be compatible with other frame types
motor_out[AP_MOTORS_MOT_1] = _servo_1->radio_out;
motor_out[AP_MOTORS_MOT_2] = _servo_2->radio_out;
motor_out[AP_MOTORS_MOT_3] = _servo_3->radio_out;
motor_out[AP_MOTORS_MOT_4] = _servo_4->radio_out;
// output gyro value
if( ext_gyro_enabled ) {
hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain);
}
}
/*
setup mixer on PX4 so that if FMU dies the pilot gets manual control
*/
bool Plane::setup_failsafe_mixing(void)
{
const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX";
bool ret = false;
char *buf = NULL;
const uint16_t buf_size = 2048;
uint16_t fileSize, new_crc;
int px4io_fd = -1;
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
buf = (char *)malloc(buf_size);
if (buf == NULL) {
goto failed;
}
fileSize = create_mixer(buf, buf_size, mixer_filename);
if (!fileSize) {
hal.console->printf("Unable to create mixer\n");
goto failed;
}
new_crc = crc_calculate((uint8_t *)buf, fileSize);
if ((int32_t)new_crc == last_mixer_crc) {
free(buf);
return true;
} else {
last_mixer_crc = new_crc;
}
px4io_fd = open("/dev/px4io", 0);
if (px4io_fd == -1) {
// px4io isn't started, no point in setting up a mixer
goto failed;
}
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
// make sure the throttle has a non-zero failsafe value before we
// disable safety. This prevents sending zero PWM during switch over
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
}
// we need to force safety on to allow us to load a mixer. We call
// it twice as there have been reports that this call can fail
// with a small probability
hal.rcout->force_safety_on();
hal.rcout->force_safety_no_wait();
/* reset any existing mixer in px4io. This shouldn't be needed,
* but is good practice */
if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) {
hal.console->printf("Unable to reset mixer\n");
goto failed;
}
/* pass the buffer to the device */
if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) {
hal.console->printf("Unable to send mixer to IO\n");
goto failed;
}
// setup RC config for each channel based on user specified
// mix/max/trim. We only do the first 8 channels due to
// a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
for (uint8_t i=0; i<8; i++) {
RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == NULL) {
continue;
}
struct pwm_output_rc_config config;
/*
we use a min/max of 900/2100 to allow for pass-thru of
larger values than the RC min/max range. This mimics the APM
behaviour of pass-thru in manual, which allows for dual-rate
transmitter setups in manual mode to go beyond the ranges
used in stabilised modes
*/
config.channel = i;
config.rc_min = 900;
config.rc_max = 2100;
if (rcmap.throttle()-1 == i) {
// throttle uses a trim of 1500, so we don't get division
// by small numbers near RC3_MIN
config.rc_trim = 1500;
} else {
config.rc_trim = constrain_int16(ch->get_radio_trim(), config.rc_min+1, config.rc_max-1);
}
config.rc_dz = 0; // zero for the purposes of manual takeover
// we set reverse as false, as users of ArduPilot will have
// input reversed on transmitter, so from the point of view of
// the mixer the input is never reversed. The one exception is
// the 2nd channel, which is reversed inside the PX4IO code,
// so needs to be unreversed here to give sane behaviour.
if (i == 1) {
config.rc_reverse = true;
} else {
config.rc_reverse = false;
}
if (i+1 == g.override_channel.get()) {
/*
This is an OVERRIDE_CHAN channel. We want IO to trigger
override with a channel input of over 1750. The px4io
code is setup for triggering below 80% of the range below
trim. To map this to values above 1750 we need to reverse
the direction and set the rc range for this channel to 1000
to 1813 (1812.5 = 1500 + 250/0.8)
*/
config.rc_assignment = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
config.rc_reverse = true;
config.rc_max = 1813; // round 1812.5 up to grant > 1750
config.rc_min = 1000;
config.rc_trim = 1500;
} else {
config.rc_assignment = i;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) {
hal.console->printf("SET_RC_CONFIG failed\n");
goto failed;
}
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
pwm_values.values[i] = 900;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) {
hal.console->printf("SET_MIN_PWM failed\n");
goto failed;
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
pwm_values.values[i] = 2100;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) {
hal.console->printf("SET_MAX_PWM failed\n");
goto failed;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0) != 0) {
hal.console->printf("SET_OVERRIDE_OK failed\n");
goto failed;
}
// setup for immediate manual control if FMU dies
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1) != 0) {
hal.console->printf("SET_OVERRIDE_IMMEDIATE failed\n");
goto failed;
}
ret = true;
failed:
if (buf != NULL) {
free(buf);
}
if (px4io_fd != -1) {
close(px4io_fd);
}
// restore safety state if it was previously armed
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
hal.rcout->force_safety_off();
}
if (!ret) {
// clear out the mixer CRC so that we will attempt to send it again
last_mixer_crc = -1;
}
return ret;
}
/*
create a PX4 mixer buffer given the current fixed wing parameters, returns the size of the buffer used
*/
uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename)
{
char *buf0 = buf;
uint16_t buf_size0 = buf_size;
/*
this is the equivalent of channel_output_mixer()
*/
const int8_t mixmul[5][2] = { { 0, 0 }, { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }};
// these are the internal clipping limits. Use scale_max1 when
// clipping to user specified min/max is wanted. Use scale_max2
// when no clipping is wanted (simulated by setting a very large
// clipping value)
const float scale_max1 = 10000;
const float scale_max2 = 1000000;
// range for mixers
const uint16_t mix_max = scale_max1 * g.mixing_gain;
// scaling factors used by PX4IO between pwm and internal values,
// as configured in setup_failsafe_mixing() below
const float pwm_min = PX4_LIM_RC_MIN;
const float pwm_max = PX4_LIM_RC_MAX;
const float pwm_scale = 2*scale_max1/(pwm_max - pwm_min);
for (uint8_t i=0; i<8; i++) {
int32_t c1, c2, mix=0;
bool rev = false;
RC_Channel_aux::Aux_servo_function_t function = RC_Channel_aux::channel_function(i);
if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
// first channel of VTAIL mix
c1 = rcmap.yaw()-1;
c2 = i;
rev = false;
mix = -mix_max*mixmul[g.vtail_output][0];
} else if (i == rcmap.yaw()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) {
// second channel of VTAIL mix
c1 = rcmap.pitch()-1;
c2 = i;
rev = true;
mix = mix_max*mixmul[g.vtail_output][1];
} else if (i == rcmap.roll()-1 && g.elevon_output > MIXING_DISABLED &&
g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) {
// first channel of ELEVON mix
c1 = i;
c2 = rcmap.pitch()-1;
rev = true;
mix = mix_max*mixmul[g.elevon_output][1];
} else if (i == rcmap.pitch()-1 && g.elevon_output > MIXING_DISABLED &&
g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) {
// second channel of ELEVON mix
c1 = i;
c2 = rcmap.roll()-1;
rev = false;
mix = mix_max*mixmul[g.elevon_output][0];
} else if (function == RC_Channel_aux::k_aileron ||
function == RC_Channel_aux::k_flaperon1 ||
function == RC_Channel_aux::k_flaperon2) {
// a secondary aileron. We don't mix flap input in yet for flaperons
c1 = rcmap.roll()-1;
} else if (function == RC_Channel_aux::k_elevator) {
// a secondary elevator
c1 = rcmap.pitch()-1;
} else if (function == RC_Channel_aux::k_rudder ||
function == RC_Channel_aux::k_steering) {
// a secondary rudder or wheel
c1 = rcmap.yaw()-1;
} else if (g.flapin_channel > 0 &&
(function == RC_Channel_aux::k_flap ||
function == RC_Channel_aux::k_flap_auto)) {
// a flap output channel, and we have a manual flap input channel
c1 = g.flapin_channel-1;
} else if (i < 4 ||
function == RC_Channel_aux::k_elevator_with_input ||
function == RC_Channel_aux::k_aileron_with_input ||
function == RC_Channel_aux::k_manual) {
// a pass-thru channel
c1 = i;
} else {
// a empty output
if (!print_buffer(buf, buf_size, "Z:\n")) {
return 0;
}
continue;
}
if (mix == 0) {
// pass through channel, possibly with reversal. We also
// adjust the gain based on the range of input and output
// channels and adjust for trims
const RC_Channel *chan1 = RC_Channel::rc_channel(i);
const RC_Channel *chan2 = RC_Channel::rc_channel(c1);
int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim());
int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim());
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
// if the input and output channels are the same then we
// apply clipping. This allows for direct pass-thru
int32_t limit = (c1==i?scale_max2:scale_max1);
int32_t in_scale_low;
if (chan2_trim <= chan2->get_radio_min()) {
in_scale_low = scale_max1;
} else {
in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->get_radio_min());
}
int32_t in_scale_high;
if (chan2->get_radio_max() <= chan2_trim) {
in_scale_high = scale_max1;
} else {
in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->get_radio_max() - chan2_trim);
}
if (chan1->get_reverse() != chan2->get_reverse()) {
in_scale_low = -in_scale_low;
in_scale_high = -in_scale_high;
}
if (!print_buffer(buf, buf_size, "M: 1\n") ||
!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n",
(int)(pwm_scale*(chan1_trim - chan1->get_radio_min())),
(int)(pwm_scale*(chan1->get_radio_max() - chan1_trim)),
(int)(pwm_scale*(chan1_trim - 1500)),
(int)-scale_max2, (int)scale_max2) ||
!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1,
in_scale_low,
in_scale_high,
0,
-limit, limit)) {
return 0;
}
} else {
const RC_Channel *chan1 = RC_Channel::rc_channel(c1);
const RC_Channel *chan2 = RC_Channel::rc_channel(c2);
int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim());
int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim());
chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1);
// mix of two input channels to give an output channel. To
// make the mixer match the behaviour of APM we need to
// scale and offset the input channels to undo the affects
// of the PX4IO input processing
if (!print_buffer(buf, buf_size, "M: 2\n") ||
!print_buffer(buf, buf_size, "O: %d %d 0 %d %d\n", mix, mix, (int)-scale_max1, (int)scale_max1)) {
return 0;
}
int32_t in_scale_low = pwm_scale*(chan1_trim - pwm_min);
int32_t in_scale_high = pwm_scale*(pwm_max - chan1_trim);
int32_t offset = pwm_scale*(chan1_trim - 1500);
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
c1, in_scale_low, in_scale_high, offset,
(int)-scale_max2, (int)scale_max2)) {
return 0;
}
in_scale_low = pwm_scale*(chan2_trim - pwm_min);
in_scale_high = pwm_scale*(pwm_max - chan2_trim);
offset = pwm_scale*(chan2_trim - 1500);
if (rev) {
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
c2, in_scale_low, in_scale_high, offset,
(int)-scale_max2, (int)scale_max2)) {
return 0;
}
} else {
if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n",
c2, -in_scale_low, -in_scale_high, -offset,
(int)-scale_max2, (int)scale_max2)) {
return 0;
}
}
}
}
/*
if possible, also write to a file for debugging purposes
*/
int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (mix_fd != -1) {
write(mix_fd, buf0, buf_size0 - buf_size);
close(mix_fd);
}
return buf_size0 - buf_size;
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min_pwm = _rc_throttle->radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _rc_throttle->radio_max; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits
int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
int16_t rpy_low = 0; // lowest motor value
int16_t rpy_high = 0; // highest motor value
int16_t yaw_allowed; // amount of yaw we can fit in
int16_t thr_adj; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
// To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
if (_rc_throttle->servo_out < 0) {
_rc_throttle->servo_out = 0;
limit.throttle_lower = true;
}
if (_rc_throttle->servo_out > _max_throttle) {
_rc_throttle->servo_out = _max_throttle;
limit.throttle_upper = true;
}
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
_rc_yaw->calc_pwm();
// if we are not sending a throttle output, we cut the motors
if (_rc_throttle->servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0;
}
if (_spin_when_armed_ramped > _min_throttle) {
_spin_when_armed_ramped = _min_throttle;
}
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
// spin motors at minimum
if (motor_enabled[i]) {
motor_out[i] = _rc_throttle->radio_min + _spin_when_armed_ramped;
}
}
// Every thing is limited
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
} else {
// check if throttle is below limit
if (_rc_throttle->radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true;
}
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = _rc_roll->pwm_out * _roll_factor[i] +
_rc_pitch->pwm_out * _pitch_factor[i];
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
rpy_low = rpy_out[i];
}
// record highest roll pich command
if (rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. mid throttle - average of highest and lowest motor (this would give the maximum possible room margin above the highest motor and below the lowest)
// 2. the higher of:
// a) the pilot's throttle input
// b) the mid point between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it
int16_t motor_mid = (rpy_low+rpy_high)/2;
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle->radio_out, (_rc_throttle->radio_out+_hover_out)/2));
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2;
yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);
if (_rc_yaw->pwm_out >= 0) {
// if yawing right
if (yaw_allowed > _rc_yaw->pwm_out) {
yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}else{
// if yawing left
yaw_allowed = -yaw_allowed;
if( yaw_allowed < _rc_yaw->pwm_out ) {
yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}
// add yaw to intermediate numbers for each motor
rpy_low = 0;
rpy_high = 0;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = rpy_out[i] +
yaw_allowed * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if( rpy_out[i] < rpy_low ) {
rpy_low = rpy_out[i];
}
// record highest roll+pitch+yaw command
if( rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// check everything fits
thr_adj = _rc_throttle->radio_out - out_best_thr_pwm;
// calc upper and lower limits of thr_adj
int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0);
// if we are increasing the throttle (situation #2 above)..
if (thr_adj > 0) {
// increase throttle as close as possible to requested throttle
// without going over out_max_pwm
if (thr_adj > thr_adj_max){
thr_adj = thr_adj_max;
// we haven't even been able to apply full throttle command
limit.throttle_upper = true;
}
}else if(thr_adj < 0){
// decrease throttle as close as possible to requested throttle
// without going under out_min_pwm or over out_max_pwm
// earlier code ensures we can't break both boundaries
int16_t thr_adj_min = min(out_min_pwm-(out_best_thr_pwm+rpy_low),0);
if (thr_adj > thr_adj_max) {
thr_adj = thr_adj_max;
limit.throttle_upper = true;
}
if (thr_adj < thr_adj_min) {
thr_adj = thr_adj_min;
limit.throttle_lower = true;
}
}
// do we need to reduce roll, pitch, yaw command
// earlier code does not allow both limit's to be passed simultainiously with abs(_yaw_factor)<1
if ((rpy_low+out_best_thr_pwm)+thr_adj < out_min_pwm){
rpy_scale = (float)(out_min_pwm-thr_adj-out_best_thr_pwm)/rpy_low;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}else if((rpy_high+out_best_thr_pwm)+thr_adj > out_max_pwm){
rpy_scale = (float)(out_max_pwm-thr_adj-out_best_thr_pwm)/rpy_high;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = out_best_thr_pwm+thr_adj +
rpy_scale*rpy_out[i];
}
}
// adjust for throttle curve
if (_throttle_curve_enabled) {
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _throttle_curve.get_y(motor_out[i]);
}
}
}
//add tbratio
for( i = 4; i<AP_MOTORS_MAX_NUM_MOTORS; i++){
if (motor_enabled[i]){
motor_out[i] *= _tb_ratio;
}
}
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
}
}
}
// send output to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->write(_motor_to_channel_map[i], motor_out[i]);
}
}
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void) {
static uint16_t last_throttle;
bool apply_skid_mix = true; // Normaly true, false when the mixage is done by the controler with skid_steer_in = 1
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->read());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->read());
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim());
// suppress steer if in failsafe and manual and skid steer mode
if (g.skid_steer_out) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim());
}
}
if (g.skid_steer_in) {
apply_skid_mix = false;
}
} else {
if (in_reverse) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-g.throttle_max,
-g.throttle_min));
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
g.throttle_min.get(),
g.throttle_max.get()));
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
if (!hal.util->get_soft_armed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
// limit throttle movement speed
throttle_slew_limit(last_throttle);
}
// record last throttle before we apply skid steering
SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle);
if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
const float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
float motor1 = throttle_scaled + 0.5f * steering_scaled;
float motor2 = throttle_scaled - 0.5f * steering_scaled;
if (!apply_skid_mix) { // Mixage is already done by a controller so just pass the value to motor
motor1 = steering_scaled;
motor2 = throttle_scaled;
} else if (fabsf(throttle_scaled) <= 0.01f) { // Use full range for on spot turn
motor1 = steering_scaled;
motor2 = -steering_scaled;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2);
}
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);
if (g.skid_steer_out) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, 0);
}
break;
case AP_Arming::YES_MIN_PWM:
default:
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim());
if (g.skid_steer_out) {
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim());
}
break;
}
}
SRV_Channels::calc_pwm();
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
SRV_Channels::output_ch_all();
#endif
}
// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{
// swash servo initialisation
_servo_1->set_range(0,1000);
_servo_2->set_range(0,1000);
_servo_3->set_range(0,1000);
_servo_4->set_angle(4500);
// ensure _coll values are reasonable
if( collective_min >= collective_max ) {
collective_min = 1000;
collective_max = 2000;
}
collective_mid = constrain_int16(collective_mid, collective_min, collective_max);
// calculate throttle mid point
throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0f;
// determine roll, pitch and throttle scaling
_roll_scaler = (float)roll_max/4500.0f;
_pitch_scaler = (float)pitch_max/4500.0f;
_collective_scalar = ((float)(collective_max-collective_min))/1000.0f;
_stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0f;
if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle));
_rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle));
_rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle));
// pitch factors
_pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle));
_pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle));
_pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
// servo min/max values
_servo_1->radio_min = 1000;
_servo_1->radio_max = 2000;
_servo_2->radio_min = 1000;
_servo_2->radio_max = 2000;
_servo_3->radio_min = 1000;
_servo_3->radio_max = 2000;
// mark swash as initialised
_swash_initialised = true;
}
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
return constrain_int16((_throttle_radio_min + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
( _throttle_radio_max - (_throttle_radio_min + _min_throttle))), _throttle_radio_min + _min_throttle, _throttle_radio_max);
}
/*
return throttle percentage from 0 to 100
*/
uint8_t Plane::throttle_percentage(void)
{
// to get the real throttle we need to use norm_output() which
// returns a number from -1 to 1.
return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100);
}
/*
calculate the throtte for auto-throttle modes
*/
void Rover::calc_throttle(float target_speed) {
// If not autostarting OR we are loitering at a waypoint
// then set the throttle to minimum
if (!auto_check_trigger() || in_stationary_loiter()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
// Stop rotation in case of loitering and skid steering
if (g.skid_steer_out) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
return;
}
const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
const int throttle_target = throttle_base + throttle_nudge;
/*
reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage.
*/
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
// use g.speed_turn_gain for a 90 degree turn, and in proportion
// for other turn angles
const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
float reduction = 1.0f - steer_rate * speed_turn_reduction;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
const float reduction2 = 1.0f - speed_turn_reduction;
if (reduction2 < reduction) {
reduction = reduction2;
}
}
// reduce the target speed by the reduction factor
target_speed *= reduction;
groundspeed_error = fabsf(target_speed) - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
// also reduce the throttle by the reduction factor. This gives a
// much faster response in turns
throttle *= reduction;
if (in_reverse) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(throttle, g.throttle_min, g.throttle_max));
}
if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
// the user has asked to use reverse throttle to brake. Apply
// it in proportion to the ground speed error, but only when
// our ground speed error is more than BRAKING_SPEEDERR.
//
// We use a linear gain, with 0 gain at a ground speed error
// of braking_speederr, and 100% gain when groundspeed_error
// is 2*braking_speederr
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
// temporarily set us in reverse to allow the PWM setting to
// go negative
set_reverse(true);
}
if (use_pivot_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
}
