From 2f4661c52f6cf6d2f360d70f5a9e2f81f5b31552 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 18 Sep 2021 19:02:12 +0100 Subject: [PATCH] Plane: use float for set/get output scaled --- ArduPlane/Attitude.cpp | 10 +++++----- ArduPlane/GCS_Plane.cpp | 2 +- ArduPlane/Log.cpp | 26 +++++++++++++------------- ArduPlane/afs_plane.cpp | 4 ++-- ArduPlane/defines.h | 2 +- ArduPlane/failsafe.cpp | 12 ++++++------ ArduPlane/mode_auto.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 2 +- ArduPlane/radio.cpp | 6 +++--- ArduPlane/servos.cpp | 34 +++++++++++++++++----------------- ArduPlane/tailsitter.cpp | 12 ++++++------ 11 files changed, 56 insertions(+), 56 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 967eebe25e..eb6e40a83d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -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; } diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index 633b93451d..7b66fcdf54 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -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; } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index dcce8f6366..1fa3065244 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -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. diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 12b4e67f51..019d867376 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -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); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index cc32ceafec..2533661a24 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -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 diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index b72c1b554e..d121c6cc30 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -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); diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index b050a02005..bbd9d304f0 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -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(); } diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index e30dbf0164..d1eb42a4bc 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -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 { diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 75cc9b4ece..0023c89fe1 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -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); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 2d4a676f07..ad63e1649e 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -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()) { diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index bf7de01605..2caf9383dd 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -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