Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: Thrust Vector output

Browse Source
This commit is contained in:
Leonard Hall 2021-04-13 14:42:48 +09:30 committed by Randy Mackay
parent 1fb2a95486
commit bb46f7a947
2 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1085,6 +1085,14 @@ void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cms
accel_y_cmss = (GRAVITY_MSS * 100) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.5f); accel_y_cmss = (GRAVITY_MSS * 100) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.5f);
} }
// returns the NED target acceleration vector for attitude control
Vector3f AC_PosControl::get_thrust_vector() const
{
Vector3f accel_target = get_accel_target();
accel_target.z = -GRAVITY_MSS * 100.0f;
return accel_target;
}
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{ {

1
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -272,6 +272,7 @@ public:
/// get desired roll, pitch which should be fed into stabilize controllers /// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _roll_target; } float get_roll() const { return _roll_target; }
float get_pitch() const { return _pitch_target; } float get_pitch() const { return _pitch_target; }
Vector3f get_thrust_vector() const;
// get_leash_xy - returns horizontal leash length in cm // get_leash_xy - returns horizontal leash length in cm
float get_leash_xy() const { return _leash; } float get_leash_xy() const { return _leash; }