/* this gives the user control of the aircraft in stabilization modes */ static void stabilize_stick_mixing() { if (!stick_mixing_enabled() || control_mode == FLY_BY_WIRE_A || control_mode == FLY_BY_WIRE_B || control_mode == TRAINING) { return; } // do stick mixing on aileron/elevator float ch1_inf; float ch2_inf; ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim; ch1_inf = fabs(ch1_inf); ch1_inf = min(ch1_inf, 400.0); ch1_inf = ((400.0 - ch1_inf) /400.0); ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim; ch2_inf = fabs(ch2_inf); ch2_inf = min(ch2_inf, 400.0); ch2_inf = ((400.0 - ch2_inf) /400.0); // scale the sensor input based on the stick input // ----------------------------------------------- g.channel_roll.servo_out *= ch1_inf; g.channel_pitch.servo_out *= ch2_inf; // Mix in stick inputs // ------------------- g.channel_roll.servo_out += g.channel_roll.pwm_to_angle(); g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle(); }
void Plane::read_radio() { if (!RC_Channels::read_input()) { control_failsafe(); return; } if(!failsafe.rc_failsafe) { failsafe.AFS_last_valid_rc_ms = millis(); } failsafe.last_valid_rc_ms = millis(); if (control_mode == TRAINING) { // in training mode we don't want to use a deadzone, as we // want manual pass through when not exceeding attitude limits channel_roll->recompute_pwm_no_deadzone(); channel_pitch->recompute_pwm_no_deadzone(); channel_throttle->recompute_pwm_no_deadzone(); channel_rudder->recompute_pwm_no_deadzone(); } control_failsafe(); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); if (g.throttle_nudge && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 50 && geofence_stickmixing()) { float nudge = (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; } } else { airspeed_nudge_cm = 0; throttle_nudge = 0; } rudder_arm_disarm_check(); if (g.rudder_only != 0) { // in rudder only mode we discard rudder input and get target // attitude from the roll channel. rudder_input = 0; } else if (stick_mixing_enabled()) { rudder_input = channel_rudder->get_control_in(); } else { // no stick mixing rudder_input = 0; } // potentially swap inputs for tailsitters quadplane.tailsitter_check_input(); // check for transmitter tuning changes tuning.check_input(control_mode); }
/* calculate yaw control for ground steering with specific course */ void Plane::calc_nav_yaw_course(void) { // holding a specific navigation course on the ground. Used in // auto-takeoff and landing int32_t bearing_error_cd = nav_controller->bearing_error_cd(); steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); if (stick_mixing_enabled()) { stick_mix_channel(channel_rudder, steering_control.steering); } steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); }
/* this gives the user control of the aircraft in stabilization modes */ void Plane::stabilize_stick_mixing_direct() { if (!stick_mixing_enabled() || control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || control_mode == TRAINING) { return; } stick_mix_channel(channel_roll, channel_roll->servo_out); stick_mix_channel(channel_pitch, channel_pitch->servo_out); }
/* this gives the user control of the aircraft in stabilization modes using FBW style controls */ void Plane::stabilize_stick_mixing_fbw() { if (!stick_mixing_enabled() || control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer == 42)) { return; } // do FBW style stick mixing. We don't treat it linearly // however. For inputs up to half the maximum, we use linear // addition to the nav_roll and nav_pitch. Above that it goes // non-linear and ends up as 2x the maximum, to ensure that // the user can direct the plane in any direction with stick // mixing. float roll_input = channel_roll->norm_input(); if (roll_input > 0.5f) { roll_input = (3*roll_input - 1); } else if (roll_input < -0.5f) { roll_input = (3*roll_input + 1); } nav_roll_cd += roll_input * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0.5f) { pitch_input = (3*pitch_input - 1); } else if (pitch_input < -0.5f) { pitch_input = (3*pitch_input + 1); } if (fly_inverted()) { pitch_input = -pitch_input; } if (pitch_input > 0) { nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; } else { nav_pitch_cd += -(pitch_input * pitch_limit_min_cd); } nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); }
/* this gives the user control of the aircraft in stabilization modes */ void Plane::stabilize_stick_mixing_direct() { if (!stick_mixing_enabled() || control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || control_mode == TRAINING) { return; } stick_mix_channel(channel_roll); stick_mix_channel(channel_pitch); }
/* stabilize the yaw axis */ static void stabilize_yaw(float speed_scaler) { float ch4_inf = 1.0; if (stick_mixing_enabled()) { // stick mixing performed for rudder for all cases including FBW // important for steering on the ground during landing // ----------------------------------------------- ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = fabs(ch4_inf); ch4_inf = min(ch4_inf, 400.0); ch4_inf = ((400.0 - ch4_inf) /400.0); } // Apply output to Rudder calc_nav_yaw(speed_scaler, ch4_inf); g.channel_rudder.servo_out *= ch4_inf; g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle(); }
int16_t Plane::rudder_input(void) { if (g.rudder_only != 0) { // in rudder only mode we discard rudder input and get target // attitude from the roll channel. return 0; } if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) && !(control_mode == MANUAL || control_mode == STABILIZE || control_mode == ACRO)) { // the user does not want any input except in these modes return 0; } if (stick_mixing_enabled()) { return channel_rudder->get_control_in(); } return 0; }
/* this gives the user control of the aircraft in stabilization modes */ void Plane::stabilize_stick_mixing_direct() { if (!stick_mixing_enabled() || control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || control_mode == TRAINING) { return; } int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); stick_mix_channel(channel_roll, aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); stick_mix_channel(channel_pitch, elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); }
void Plane::read_radio() { if (!hal.rcin->new_input()) { control_failsafe(channel_throttle->get_radio_in()); return; } if(!failsafe.ch3_failsafe) { failsafe.AFS_last_valid_rc_ms = millis(); } failsafe.last_valid_rc_ms = millis(); elevon.ch1_temp = channel_roll->read(); elevon.ch2_temp = channel_pitch->read(); uint16_t pwm_roll, pwm_pitch; if (g.mix_mode == 0) { pwm_roll = elevon.ch1_temp; pwm_pitch = elevon.ch2_temp; }else{ pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; } RC_Channel::set_pwm_all(); if (control_mode == TRAINING) { // in training mode we don't want to use a deadzone, as we // want manual pass through when not exceeding attitude limits channel_roll->set_pwm_no_deadzone(pwm_roll); channel_pitch->set_pwm_no_deadzone(pwm_pitch); channel_throttle->set_pwm_no_deadzone(channel_throttle->read()); channel_rudder->set_pwm_no_deadzone(channel_rudder->read()); } else { channel_roll->set_pwm(pwm_roll); channel_pitch->set_pwm(pwm_pitch); } control_failsafe(channel_throttle->get_radio_in()); channel_throttle->set_servo_out(channel_throttle->get_control_in()); if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) { float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; } } else { airspeed_nudge_cm = 0; throttle_nudge = 0; } rudder_arm_disarm_check(); if (g.rudder_only != 0) { // in rudder only mode we discard rudder input and get target // attitude from the roll channel. rudder_input = 0; } else if (stick_mixing_enabled()) { rudder_input = channel_rudder->get_control_in(); } else { // no stick mixing rudder_input = 0; } // check for transmitter tuning changes tuning.check_input(control_mode); }