Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: add get_vel_target and get_accel_target

This commit is contained in:
Randy Mackay 2024-12-16 20:45:50 +09:00 committed by Peter Barker
parent f02b1ddea2
commit 74c702674b
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1208,6 +1208,28 @@ bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &v
accel_offset_NED.z = -_accel_offset_target.z * 0.01;
return true;
}
// get target velocity in m/s in NED frame
bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED)
{
if (!is_active_xy() || !is_active_z()) {
return false;
}
vel_target_NED.xy() = _vel_target.xy() * 0.01;
vel_target_NED.z = -_vel_target.z * 0.01;
return true;
}
// get target acceleration in m/s/s in NED frame
bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED)
{
if (!is_active_xy() || !is_active_z()) {
return false;
}
accel_target_NED.xy() = _accel_target.xy() * 0.01;
accel_target_NED.z = -_accel_target.z * 0.01;
return true;
}
#endif
/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -339,6 +339,12 @@ public:
// units are m, m/s and m/s/s in NED frame
bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED);
bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED);
// get target velocity in m/s in NED frame
bool get_vel_target(Vector3f &vel_target_NED);
// get target acceleration in m/s/s in NED frame
bool get_accel_target(Vector3f &accel_target_NED);
#endif
/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame