fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 11:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_attitude only based yaw control fix (#10803)

Browse Source 
* Add attitude sub to mc_pos_control for yaw usage.

* Initialize pos control _states struct.

* Remove unnecessary init for struct in mc pos ctrl.

* Only use att topic for yaw setting in FlightTask.
This commit is contained in:
James Goppert
2018-11-06 12:03:13 -05:00
committed by GitHub
parent 40884a9238
commit c642025339
2 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+5 -3
View File
@@ -90,6 +90,11 @@ void FlightTask::_evaluateVehicleLocalPosition()
_yaw = NAN;
_dist_to_bottom = NAN;
if ((_time_stamp_current - _sub_attitude->get().timestamp) < _timeout) {
// yaw
_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().q)).psi();
}
// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
@@ -113,9 +118,6 @@ void FlightTask::_evaluateVehicleLocalPosition()
_velocity(2) = _sub_vehicle_local_position->get().vz;
}
// yaw
_yaw = _sub_vehicle_local_position->get().yaw;
// distance to bottom
if (_sub_vehicle_local_position->get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {