Skip to content

Commit

Permalink
Plane: Converted demanded_pitch_cd to float degrees
Browse files Browse the repository at this point in the history
Also initialized to NaN which will not show up in the ATT.DesPitch
plots, unless a mode (usually) overwrites it.
  • Loading branch information
Georacer committed Jan 21, 2025
1 parent 72a3e2e commit 58218d6
Show file tree
Hide file tree
Showing 4 changed files with 10 additions and 6 deletions.
6 changes: 3 additions & 3 deletions ArduPlane/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)));
}

Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/Log.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
};

Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/Plane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down

0 comments on commit 58218d6

Please sign in to comment.