From 56b2fd0257b6739de8a2308bab10e7142277ab40 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 21 Apr 2016 10:24:28 +0200 Subject: [PATCH] fixed rover example --- src/examples/rover_steering_control/main.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 563d6b0cca..abf3c94d74 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -66,6 +66,7 @@ #include #include #include +#include /* process-specific header files */ #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 */ - float yaw_err = att->yaw - att_sp->yaw_body; + matrix::Quaternion q(&att->q[0]); + matrix::Euler euler(q); + matrix::Quaternion q_sp(&att_sp->q_d[0]); + matrix::Euler euler_sp(q_sp); + float yaw_err = euler(2) - euler_sp(2); actuators->control[2] = yaw_err * pp.yaw_p; /* 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); /* sanity check and publish actuator outputs */ - if (isfinite(actuators.control[0]) && - isfinite(actuators.control[1]) && - isfinite(actuators.control[2]) && - isfinite(actuators.control[3])) { + if (PX4_ISFINITE(actuators.control[0]) && + PX4_ISFINITE(actuators.control[1]) && + PX4_ISFINITE(actuators.control[2]) && + PX4_ISFINITE(actuators.control[3])) { orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); if (verbose) {