Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 14:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: use float for set/get output scaled

Browse Source
This commit is contained in:
Iampete1 2021-09-18 19:02:12 +01:00 committed by Andrew Tridgell
parent a590a675d6
commit 2f4661c52f
11 changed files with 56 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/Attitude.cpp
View File

@ -225,11 +225,11 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == &mode_training) { control_mode == &mode_training) {
return; return;
} }
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
aileron = channel_roll->stick_mixing(aileron); aileron = channel_roll->stick_mixing(aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
elevator = channel_pitch->stick_mixing(elevator); elevator = channel_pitch->stick_mixing(elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
} }
@ -379,8 +379,8 @@ void Plane::stabilize_acro(float speed_scaler)
{ {
const float rexpo = roll_in_expo(true); const float rexpo = roll_in_expo(true);
const float pexpo = pitch_in_expo(true); const float pexpo = pitch_in_expo(true);
float roll_rate = (rexpo/float(SERVO_MAX)) * g.acro_roll_rate; float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate;
float pitch_rate = (pexpo/float(SERVO_MAX)) * g.acro_pitch_rate; float pitch_rate = (pexpo/SERVO_MAX) * g.acro_pitch_rate;
/* /*
check for special roll handling near the pitch poles check for special roll handling near the pitch poles
@ -545,7 +545,7 @@ void Plane::calc_throttle()
// user has asked for zero throttle - this may be done by a // user has asked for zero throttle - this may be done by a
// mission which wants to turn off the engine for a parachute // mission which wants to turn off the engine for a parachute
// landing // landing
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
return; return;
} }

2
ArduPlane/GCS_Plane.cpp
View File

@ -24,7 +24,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
if (plane.have_reverse_thrust()) { if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR; control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
} }
if (plane.have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) { if (plane.have_reverse_thrust() && is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR; control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR; control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
} }

26
ArduPlane/Log.cpp
View File

@ -88,9 +88,9 @@ struct PACKED log_Control_Tuning {
int16_t roll; int16_t roll;
int16_t nav_pitch_cd; int16_t nav_pitch_cd;
int16_t pitch; int16_t pitch;
int16_t throttle_out; float throttle_out;
int16_t rudder_out; float rudder_out;
int16_t throttle_dem; float throttle_dem;
float airspeed_estimate; float airspeed_estimate;
float synthetic_airspeed; float synthetic_airspeed;
float EAS2TAS; float EAS2TAS;
@ -115,9 +115,9 @@ void Plane::Log_Write_Control_Tuning()
roll : (int16_t)ahrs.roll_sensor, roll : (int16_t)ahrs.roll_sensor,
nav_pitch_cd : (int16_t)nav_pitch_cd, nav_pitch_cd : (int16_t)nav_pitch_cd,
pitch : (int16_t)ahrs.pitch_sensor, pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), throttle_out : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand(), throttle_dem : SpdHgt_Controller->get_throttle_demand(),
airspeed_estimate : est_airspeed, airspeed_estimate : est_airspeed,
synthetic_airspeed : synthetic_airspeed, synthetic_airspeed : synthetic_airspeed,
EAS2TAS : ahrs.get_EAS2TAS(), EAS2TAS : ahrs.get_EAS2TAS(),
@ -228,11 +228,11 @@ void Plane::Log_Write_Status()
struct PACKED log_AETR { struct PACKED log_AETR {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
int16_t aileron; float aileron;
int16_t elevator; float elevator;
int16_t throttle; float throttle;
int16_t rudder; float rudder;
int16_t flap; float flap;
float speed_scaler; float speed_scaler;
}; };
@ -322,7 +322,7 @@ const struct LogStructure Plane::log_structure[] = {
// @Field: GU: groundspeed undershoot when flying with minimum groundspeed // @Field: GU: groundspeed undershoot when flying with minimum groundspeed
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning), { LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qcccchhhfffi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdrOut,ThD,As,SAs,E2T,GU", "sdddd---nn-n", "FBBBB---00-B" , true }, "CTUN", "Qccccffffffi", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdrOut,ThD,As,SAs,E2T,GU", "sdddd---nn-n", "FBBBB---00-B" , true },
// @LoggerMessage: NTUN // @LoggerMessage: NTUN
// @Description: Navigation Tuning information - e.g. vehicle destination // @Description: Navigation Tuning information - e.g. vehicle destination
@ -444,7 +444,7 @@ const struct LogStructure Plane::log_structure[] = {
// @Field: Flap: Pre-mixer value for flaps output (between -4500 to 4500) // @Field: Flap: Pre-mixer value for flaps output (between -4500 to 4500)
// @Field: SS: Surface movement / airspeed scaling value // @Field: SS: Surface movement / airspeed scaling value
{ LOG_AETR_MSG, sizeof(log_AETR), { LOG_AETR_MSG, sizeof(log_AETR),
"AETR", "Qhhhhhf", "TimeUS,Ail,Elev,Thr,Rudd,Flap,SS", "s------", "F------" , true }, "AETR", "Qffffff", "TimeUS,Ail,Elev,Thr,Rudd,Flap,SS", "s------", "F------" , true },
// @LoggerMessage: OFG // @LoggerMessage: OFG
// @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s. // @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s.

4
ArduPlane/afs_plane.cpp
View File

@ -25,8 +25,8 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
plane.landing.terminate(); plane.landing.terminate();
} else { } else {
// aerodynamic termination is the default approach to termination // aerodynamic termination is the default approach to termination
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100); SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX);

2
ArduPlane/defines.h
View File

@ -7,7 +7,7 @@
#define FALSE 0 #define FALSE 0
#define DEBUG 0 #define DEBUG 0
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an #define SERVO_MAX 4500.0 // This value represents 45 degrees and is just an
// arbitrary representation of servo max travel. // arbitrary representation of servo max travel.
// failsafe // failsafe

12
ArduPlane/failsafe.cpp
View File

@ -63,10 +63,10 @@ void Plane::failsafe_check(void)
// pass RC inputs to outputs every 20ms // pass RC inputs to outputs every 20ms
RC_Channels::clear_overrides(); RC_Channels::clear_overrides();
int16_t roll = roll_in_expo(false); float roll = roll_in_expo(false);
int16_t pitch = pitch_in_expo(false); float pitch = pitch_in_expo(false);
int16_t throttle = get_throttle_input(true); float throttle = get_throttle_input(true);
int16_t rudder = rudder_in_expo(false); float rudder = rudder_in_expo(false);
if (!hal.util->get_soft_armed()) { if (!hal.util->get_soft_armed()) {
throttle = 0; throttle = 0;
@ -95,8 +95,8 @@ void Plane::failsafe_check(void)
// setup secondary output channels that do have // setup secondary output channels that do have
// corresponding input channels // corresponding input channels
SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true); SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0.0);
// setup flaperons // setup flaperons
flaperon_update(0); flaperon_update(0);

2
ArduPlane/mode_auto.cpp
View File

@ -82,7 +82,7 @@ void ModeAuto::update()
if (plane.landing.is_throttle_suppressed()) { if (plane.landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type // if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
} else { } else {
plane.calc_throttle(); plane.calc_throttle();
} }

2
ArduPlane/mode_takeoff.cpp
View File

@ -120,7 +120,7 @@ void ModeTakeoff::update()
} }
if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) { if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0);
plane.takeoff_calc_roll(); plane.takeoff_calc_roll();
plane.takeoff_calc_pitch(); plane.takeoff_calc_pitch();
} else { } else {

6
ArduPlane/radio.cpp
View File

@ -340,7 +340,7 @@ void Plane::trim_radio()
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_left); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_left);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_right); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_right);
if (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) == 0) { if (is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_rudder))) {
// trim differential spoilers if no rudder input // trim differential spoilers if no rudder input
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft1); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft1);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft2); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft2);
@ -348,8 +348,8 @@ void Plane::trim_radio()
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight2); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight2);
} }
if (SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) == 0 && if (is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)) &&
SRV_Channels::get_output_scaled(SRV_Channel::k_flap) == 0) { is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap))) {
// trim flaperons if no flap input // trim flaperons if no flap input
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right);

34
ArduPlane/servos.cpp
View File

@ -305,7 +305,7 @@ void Plane::dspoiler_update(void)
void Plane::airbrake_update(void) void Plane::airbrake_update(void)
{ {
// Calculate any manual airbrake input from RC channel option. // Calculate any manual airbrake input from RC channel option.
int8_t manual_airbrake_percent = 0; float manual_airbrake_percent = 0;
if (channel_airbrake != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) { if (channel_airbrake != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) {
manual_airbrake_percent = channel_airbrake->percent_input(); manual_airbrake_percent = channel_airbrake->percent_input();
@ -313,9 +313,9 @@ void Plane::airbrake_update(void)
// Calculate auto airbrake from negative throttle. // Calculate auto airbrake from negative throttle.
float throttle_min = aparm.throttle_min.get(); float throttle_min = aparm.throttle_min.get();
int16_t airbrake_pc = 0; float airbrake_pc = 0;
int throttle_pc = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); float throttle_pc = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (throttle_min < 0) { if (throttle_min < 0) {
if (landing.is_flaring()) { if (landing.is_flaring()) {
@ -324,7 +324,7 @@ void Plane::airbrake_update(void)
} }
else { else {
// Determine fraction between zero and full negative throttle. // Determine fraction between zero and full negative throttle.
airbrake_pc = constrain_int16(-throttle_pc, 0, 100); airbrake_pc = constrain_float(-throttle_pc, 0, 100);
} }
} }
@ -379,7 +379,7 @@ void Plane::set_servos_manual_passthrough(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_in_expo(false));
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_in_expo(false));
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_in_expo(false));
int8_t throttle = get_throttle_input(true); float throttle = get_throttle_input(true);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
@ -429,7 +429,7 @@ void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) co
max_throttle = MIN((int8_t)(ratio * (float)max_throttle), 100); max_throttle = MIN((int8_t)(ratio * (float)max_throttle), 100);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100)); constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100));
} }
/* /*
@ -442,14 +442,14 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
// overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value // 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 // throttle limit will attack by 10% per second
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0 && // demanding too much positive thrust if (is_positive(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) && // demanding too much positive thrust
throttle_watt_limit_max < max_throttle - 25 && throttle_watt_limit_max < max_throttle - 25 &&
now - throttle_watt_limit_timer_ms >= 1) { now - throttle_watt_limit_timer_ms >= 1) {
// always allow for 25% throttle available regardless of battery status // always allow for 25% throttle available regardless of battery status
throttle_watt_limit_timer_ms = now; throttle_watt_limit_timer_ms = now;
throttle_watt_limit_max++; throttle_watt_limit_max++;
} else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0 && } else if (is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) &&
min_throttle < 0 && // reverse thrust is available min_throttle < 0 && // reverse thrust is available
throttle_watt_limit_min < -(min_throttle) - 25 && throttle_watt_limit_min < -(min_throttle) - 25 &&
now - throttle_watt_limit_timer_ms >= 1) { now - throttle_watt_limit_timer_ms >= 1) {
@ -530,7 +530,7 @@ void Plane::set_servos_controlled(void)
throttle_watt_limiter(min_throttle, max_throttle); throttle_watt_limiter(min_throttle, max_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_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)); constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
if (!hal.util->get_soft_armed()) { if (!hal.util->get_soft_armed()) {
if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) { if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
@ -538,12 +538,12 @@ void Plane::set_servos_controlled(void)
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
} else { } else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
} }
} else if (suppress_throttle()) { } else if (suppress_throttle()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0 ); // default SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default
// throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines:
if (landing.is_flaring() && landing.use_thr_min_during_flare() ) { if (landing.is_flaring() && landing.use_thr_min_during_flare() ) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get());
@ -559,7 +559,7 @@ void Plane::set_servos_controlled(void)
control_mode == &mode_autotune) { control_mode == &mode_autotune) {
// a manual throttle mode // a manual throttle mode
if (failsafe.throttle_counter) { if (failsafe.throttle_counter) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
} else if (g.throttle_passthru_stabilize) { } else if (g.throttle_passthru_stabilize) {
// manual pass through of throttle while in FBWA or // manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set // STABILIZE mode with THR_PASS_STAB set
@ -575,12 +575,12 @@ void Plane::set_servos_controlled(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
} else if (quadplane.in_vtol_mode()) { } else if (quadplane.in_vtol_mode()) {
int16_t fwd_thr = 0; float fwd_thr = 0;
// if armed and not spooled down ask quadplane code for forward throttle // if armed and not spooled down ask quadplane code for forward throttle
if (quadplane.motors->armed() && if (quadplane.motors->armed() &&
quadplane.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN) { quadplane.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN) {
fwd_thr = constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle); fwd_thr = constrain_float(quadplane.forward_throttle_pct(), min_throttle, max_throttle);
} }
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
#endif // HAL_QUADPLANE_ENABLED #endif // HAL_QUADPLANE_ENABLED
@ -717,7 +717,7 @@ void Plane::servos_twin_engine_mix(void)
{ {
float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
float rud_gain = float(plane.g2.rudd_dt_gain) / 100; float rud_gain = float(plane.g2.rudd_dt_gain) / 100;
rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / float(SERVO_MAX); rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / SERVO_MAX;
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
if (afs.should_crash_vehicle()) { if (afs.should_crash_vehicle()) {

12
ArduPlane/tailsitter.cpp
View File

@ -272,7 +272,7 @@ void Tailsitter::output(void)
throttle = MAX(throttle,motors->actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01)); throttle = MAX(throttle,motors->actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01));
} }
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0.0);
plane.rudder_dt = 0; plane.rudder_dt = 0;
// in assisted flight this is done in the normal motor output path // in assisted flight this is done in the normal motor output path
@ -391,10 +391,10 @@ void Tailsitter::output(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
// Check for saturated limits // Check for saturated limits
bool tilt_lim = _is_vectored && ((labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft)) == SERVO_MAX) || (labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) == SERVO_MAX)); bool tilt_lim = _is_vectored && ((fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft)) >= SERVO_MAX) || (fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) >= SERVO_MAX));
bool roll_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_rudder)) == SERVO_MAX; bool roll_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_rudder)) >= SERVO_MAX;
bool pitch_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_elevator)) == SERVO_MAX; bool pitch_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_elevator)) >= SERVO_MAX;
bool yaw_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) == SERVO_MAX; bool yaw_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) >= SERVO_MAX;
if (roll_lim) { if (roll_lim) {
motors->limit.roll = true; motors->limit.roll = true;
@ -648,7 +648,7 @@ void Tailsitter::speed_scaling(void)
SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft, SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft,
SRV_Channel::Aux_servo_function_t::k_tiltMotorRight}; SRV_Channel::Aux_servo_function_t::k_tiltMotorRight};
for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) { for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) {
int32_t v = SRV_Channels::get_output_scaled(functions[i]); float v = SRV_Channels::get_output_scaled(functions[i]);
if ((functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft) || (functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) { if ((functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft) || (functions[i] == SRV_Channel::Aux_servo_function_t::k_tiltMotorRight)) {
// always apply throttle scaling to tilts // always apply throttle scaling to tilts
v *= throttle_scaler; v *= throttle_scaler;