Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

fixed rover example

Browse Source
This commit is contained in:
tumbili
2016-04-21 10:24:28 +02:00
committed by Lorenz Meier
parent 38b949a5aa
commit 56b2fd0257
1 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/rover_steering_control/main.cpp
+10 -5
View File
@@ -66,6 +66,7 @@
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <matrix/math.hpp>
/* process-specific header files */ /* process-specific header files */
#include "params.h" #include "params.h"
@@ -178,7 +179,11 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
/* /*
* Calculate roll error and apply P gain * Calculate roll error and apply P gain
*/ */
float yaw_err = att->yaw - att_sp->yaw_body; matrix::Quaternion<float> q(&att->q[0]);
matrix::Euler<float> euler(q);
matrix::Quaternion<float> q_sp(&att_sp->q_d[0]);
matrix::Euler<float> euler_sp(q_sp);
float yaw_err = euler(2) - euler_sp(2);
actuators->control[2] = yaw_err * pp.yaw_p; actuators->control[2] = yaw_err * pp.yaw_p;
/* copy throttle */ /* copy throttle */
@@ -354,10 +359,10 @@ int rover_steering_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* sanity check and publish actuator outputs */ /* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) && if (PX4_ISFINITE(actuators.control[0]) &&
isfinite(actuators.control[1]) && PX4_ISFINITE(actuators.control[1]) &&
isfinite(actuators.control[2]) && PX4_ISFINITE(actuators.control[2]) &&
isfinite(actuators.control[3])) { PX4_ISFINITE(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
if (verbose) { if (verbose) {