aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AR_AttitudeControl: get_throttle_speed_pid_info.FF includes base throttle

Browse Source
This commit is contained in:
Randy Mackay 2022-01-06 19:28:17 +09:00
parent c50b8597db
commit 91d40b768c
2 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -637,6 +637,10 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor
throttle_out += _throttle_speed_pid.get_ff();
throttle_out += throttle_base;
// update PID info for reporting purposes
_throttle_speed_pid_info = _throttle_speed_pid.get_pid_info();
_throttle_speed_pid_info.FF += throttle_base;
// clear local limit flags used to stop i-term build-up as we stop reversed outputs going to motors
_throttle_limit_low = false;
_throttle_limit_high = false;

3
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -80,9 +80,9 @@ public:
// low level control accessors for reporting and logging
AC_P& get_steering_angle_p() { return _steer_angle_p; }
AC_PID& get_steering_rate_pid() { return _steer_rate_pid; }
AC_PID& get_throttle_speed_pid() { return _throttle_speed_pid; }
AC_PID& get_pitch_to_throttle_pid() { return _pitch_to_throttle_pid; }
AC_PID& get_sailboat_heel_pid() { return _sailboat_heel_pid; }
const AP_Logger::PID_Info& get_throttle_speed_pid_info() const { return _throttle_speed_pid_info; }
// get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success
bool get_forward_speed(float &speed) const;
@ -143,6 +143,7 @@ private:
uint32_t _stop_last_ms; // system time the vehicle was at a complete stop
bool _throttle_limit_low; // throttle output was limited from going too low (used to reduce i-term buildup)
bool _throttle_limit_high; // throttle output was limited from going too high (used to reduce i-term buildup)
AP_Logger::PID_Info _throttle_speed_pid_info; // local copy of throttle_speed controller's PID info to allow reporting of unusual FF
// balancebot pitch control
uint32_t _balance_last_ms = 0;