From 58218d6fa19d14a7a2d9e6212bb5f8030c9f9b43 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 14 Jan 2025 13:27:15 +0100 Subject: [PATCH] Plane: Converted demanded_pitch_cd to float degrees Also initialized to NaN which will not show up in the ATT.DesPitch plots, unless a mode (usually) overwrites it. --- ArduPlane/Attitude.cpp | 6 +++--- ArduPlane/Log.cpp | 2 +- ArduPlane/Plane.cpp | 4 ++++ ArduPlane/Plane.h | 4 ++-- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index d46fa9edd56386..094e62b3e901fd 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -209,7 +209,7 @@ float Plane::stabilize_pitch_get_pitch_out() const bool quadplane_in_frwd_transition = false; #endif - demanded_pitch_cd = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; + demanded_pitch = nav_pitch_cd*0.01f + g.pitch_trim + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01f*g.kff_throttle_to_pitch; bool disable_integrator = false; if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { disable_integrator = true; @@ -224,10 +224,10 @@ float Plane::stabilize_pitch_get_pitch_out() !control_mode->does_auto_throttle() && flare_mode == FlareMode::ENABLED_PITCH_TARGET && throttle_at_zero()) { - demanded_pitch_cd = landing.get_pitch_cd(); + demanded_pitch = landing.get_pitch_cd()*0.01f; } - return pitchController.get_servo_out(demanded_pitch_cd - ahrs.pitch_sensor, speed_scaler, disable_integrator, + return pitchController.get_servo_out(demanded_pitch*100 - ahrs.pitch_sensor, speed_scaler, disable_integrator, ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index a72be66592f71b..e96c454d5e0a31 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -7,7 +7,7 @@ void Plane::Log_Write_Attitude(void) { Vector3f targets { // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude nav_roll_cd * 0.01f, - demanded_pitch_cd * 0.01f, + demanded_pitch, 0 //Plane does not have the concept of navyaw. This is a placeholder. }; diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 2fb6cb1eebdc51..0c46c13b8eea50 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -511,6 +511,10 @@ void Plane::update_control_mode(void) takeoff_state.throttle_lim_max = 100.0f; takeoff_state.throttle_lim_min = -100.0f; + // Preemptively set demanded pitch to NaN, so that it properly doesn't + // get logged. Modes ovewriting it (most modes do) will change it. + demanded_pitch = NAN; + update_fly_forward(); control_mode->update(); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 094223c11eee2a..86a315759e7a29 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -671,8 +671,8 @@ class Plane : public AP_Vehicle { // The instantaneous navigator-commanded pitch angle. Hundredths of a degree int32_t nav_pitch_cd; - // The instantaneous desired pitch angle. Hundredths of a degree. - int32_t demanded_pitch_cd; + // The instantaneous desired pitch angle in degrees. + float demanded_pitch; // the aerodynamic load factor. This is calculated from the demanded // roll before the roll is clipped, using 1/sqrt(cos(nav_roll))