Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: use float for set/get output scaled

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) {
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);
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);
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 pexpo = pitch_in_expo(true);
float roll_rate = (rexpo/float(SERVO_MAX)) * g.acro_roll_rate;
float pitch_rate = (pexpo/float(SERVO_MAX)) * g.acro_pitch_rate;
float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate;
float pitch_rate = (pexpo/SERVO_MAX) * g.acro_pitch_rate;
/*
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
// mission which wants to turn off the engine for a parachute
// landing
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
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()) {
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_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 nav_pitch_cd;
int16_t pitch;
int16_t throttle_out;
int16_t rudder_out;
int16_t throttle_dem;
float throttle_out;
float rudder_out;
float throttle_dem;
float airspeed_estimate;
float synthetic_airspeed;
float EAS2TAS;
@ -115,9 +115,9 @@ void Plane::Log_Write_Control_Tuning()
roll : (int16_t)ahrs.roll_sensor,
nav_pitch_cd : (int16_t)nav_pitch_cd,
pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
rudder_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
throttle_dem : (int16_t)SpdHgt_Controller->get_throttle_demand(),
throttle_out : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder),
throttle_dem : SpdHgt_Controller->get_throttle_demand(),
airspeed_estimate : est_airspeed,
synthetic_airspeed : synthetic_airspeed,
EAS2TAS : ahrs.get_EAS2TAS(),
@ -228,11 +228,11 @@ void Plane::Log_Write_Status()
struct PACKED log_AETR {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t aileron;
int16_t elevator;
int16_t throttle;
int16_t rudder;
int16_t flap;
float aileron;
float elevator;
float throttle;
float rudder;
float flap;
float speed_scaler;
};
@ -322,7 +322,7 @@ const struct LogStructure Plane::log_structure[] = {
// @Field: GU: groundspeed undershoot when flying with minimum groundspeed
{ 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
// @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: SS: Surface movement / airspeed scaling value
{ 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
// @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();
} else {
// 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, 100);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100.0);
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_rudder, 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 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.
// failsafe

12
ArduPlane/failsafe.cpp
View File

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

2
ArduPlane/mode_auto.cpp
View File

@ -82,7 +82,7 @@ void ModeAuto::update()
if (plane.landing.is_throttle_suppressed()) {
// 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 {
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) {
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_pitch();
} 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_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
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft1);
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);
}
if (SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) == 0 &&
SRV_Channels::get_output_scaled(SRV_Channel::k_flap) == 0) {
if (is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)) &&
is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap))) {
// 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_right);

34
ArduPlane/servos.cpp
View File

@ -305,7 +305,7 @@ void Plane::dspoiler_update(void)
void Plane::airbrake_update(void)
{
// 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) {
manual_airbrake_percent = channel_airbrake->percent_input();
@ -313,9 +313,9 @@ void Plane::airbrake_update(void)
// Calculate auto airbrake from negative throttle.
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 (landing.is_flaring()) {
@ -324,7 +324,7 @@ void Plane::airbrake_update(void)
}
else {
// 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_elevator, pitch_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);
#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);
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
// 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 &&
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 (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
throttle_watt_limit_min < -(min_throttle) - 25 &&
now - throttle_watt_limit_timer_ms >= 1) {
@ -530,7 +530,7 @@ void Plane::set_servos_controlled(void)
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));
constrain_float(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::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_throttleRight, SRV_Channel::Limit::ZERO_PWM);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
}
} 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:
if (landing.is_flaring() && landing.use_thr_min_during_flare() ) {
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) {
// a manual throttle mode
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) {
// manual pass through of throttle while in FBWA or
// 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));
#if HAL_QUADPLANE_ENABLED
} 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 (quadplane.motors->armed() &&
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);
#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 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 (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));
}
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;
// 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);
// 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 roll_lim = labs(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 yaw_lim = labs(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) == 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 = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_rudder)) >= SERVO_MAX;
bool pitch_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_elevator)) >= SERVO_MAX;
bool yaw_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) >= SERVO_MAX;
if (roll_lim) {
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_tiltMotorRight};
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)) {
// always apply throttle scaling to tilts
v *= throttle_scaler;