Browse Source

AP_InertialNav: get_hgt_ctrl_limit only modifies limit on success

master
Randy Mackay 10 years ago
parent
commit
ba94a993f5
  1. 12
      libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
  2. 2
      libraries/AP_InertialNav/AP_InertialNav_NavEKF.h

12
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp

@ -140,13 +140,15 @@ bool AP_InertialNav_NavEKF::get_hagl(float height) const @@ -140,13 +140,15 @@ bool AP_InertialNav_NavEKF::get_hagl(float height) const
* it will return invalid when no limiting is required
* @return
*/
bool AP_InertialNav_NavEKF::get_hgt_ctrl_limit(float limit) const
bool AP_InertialNav_NavEKF::get_hgt_ctrl_limit(float& limit) const
{
// true when estimate is valid
bool valid = _ahrs_ekf.get_NavEKF().getHeightControlLimit(limit);
// convert height from m to cm
limit *= 100.0f;
return valid;
if (_ahrs_ekf.get_NavEKF().getHeightControlLimit(limit)) {
// convert height from m to cm
limit *= 100.0f;
return true;
}
return false;
}
/**

2
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h

@ -100,7 +100,7 @@ public: @@ -100,7 +100,7 @@ public:
* it will return invalid when no limiting is required
* @return
*/
bool get_hgt_ctrl_limit(float limit) const;
bool get_hgt_ctrl_limit(float& limit) const;
/**
* get_velocity_z - returns the current climbrate.

Loading…
Cancel
Save