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

APM_Control: get_throttle_out_from_pitch() accepts dt as argument

Browse Source
This commit is contained in:
Ebin 2018-06-27 18:44:55 +05:30 committed by Randy Mackay
parent 10914d88b7
commit 97a260a980
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -488,7 +488,7 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
// for balancebot // for balancebot
// return a throttle output from -1 to +1, given a desired pitch angle // return a throttle output from -1 to +1, given a desired pitch angle
// desired_pitch is in radians // desired_pitch is in radians
float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool armed) float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool armed, float dt)
{ {
//reset I term and return if disarmed //reset I term and return if disarmed
@ -497,13 +497,13 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool
return 0.0f; return 0.0f;
} }
// calculate dt // sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
float dt = (now - _balance_last_ms) * 0.001f;
// if not called recently, reset input filter // if not called recently, reset input filter
if ((_balance_last_ms == 0) || (dt > (AR_ATTCONTROL_TIMEOUT_MS / 1000.0f))) { if ((_balance_last_ms == 0) || ((now - _balance_last_ms) > (AR_ATTCONTROL_TIMEOUT_MS / 1000.0f))) {
dt = 0.0f;
_pitch_to_throttle_pid.reset_filter(); _pitch_to_throttle_pid.reset_filter();
} else { } else {
_pitch_to_throttle_pid.set_dt(dt); _pitch_to_throttle_pid.set_dt(dt);

2
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -92,7 +92,7 @@ public:
// for balancebot // for balancebot
// return a throttle output from -1 to +1 given a desired pitch angle // return a throttle output from -1 to +1 given a desired pitch angle
// desired_pitch is in radians // desired_pitch is in radians
float get_throttle_out_from_pitch(float desired_pitch, bool armed); float get_throttle_out_from_pitch(float desired_pitch, bool armed, float dt);
// low level control accessors for reporting and logging // low level control accessors for reporting and logging
AC_P& get_steering_angle_p() { return _steer_angle_p; } AC_P& get_steering_angle_p() { return _steer_angle_p; }