Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

VMount: Do not rely on euler angles

Browse Source
This commit is contained in:
Lorenz Meier
2016-09-27 11:58:19 +02:00
parent affefcc75e
commit 873ed17b14
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/vmount/output.cpp
+4 -2
View File
@@ -46,6 +46,7 @@
#include <px4_defines.h> #include <px4_defines.h>
#include <geo/geo.h> #include <geo/geo.h>
#include <math.h> #include <math.h>
#include <mathlib/mathlib.h>
namespace vmount namespace vmount
{ {
@@ -186,11 +187,12 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t)
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude); orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
} }
float att[3] = { vehicle_attitude.roll, vehicle_attitude.pitch, vehicle_attitude.yaw }; matrix::Quaternion<float> q(&vehicle_attitude.q[0]);
matrix::Euler<float> euler(q);
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
if (_stabilize[i]) { if (_stabilize[i]) {
_angle_outputs[i] = _angle_setpoints[i] - att[i]; _angle_outputs[i] = _angle_setpoints[i] - euler(i);
} else { } else {
_angle_outputs[i] = _angle_setpoints[i]; _angle_outputs[i] = _angle_setpoints[i];