diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-21 14:59:55 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-21 14:59:55 +0100 |
commit | ffda224b414204af7fb0a4f660f7b72ee013bcd2 (patch) | |
tree | 3c6ac16cc721ed4daa0fc32b7709758db2f54b54 /apps/ardrone_interface | |
parent | d1cde9858d6b8d4c2fb248342e8a3e0aabefa8fd (diff) | |
download | px4-firmware-ffda224b414204af7fb0a4f660f7b72ee013bcd2.tar.gz px4-firmware-ffda224b414204af7fb0a4f660f7b72ee013bcd2.tar.bz2 px4-firmware-ffda224b414204af7fb0a4f660f7b72ee013bcd2.zip |
Added actuator controls effective topic
Diffstat (limited to 'apps/ardrone_interface')
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 32 |
1 files changed, 28 insertions, 4 deletions
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 70b8ad6de..f15c74a22 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -45,6 +45,7 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> #include <systemlib/err.h> #include "ardrone_motor_control.h" @@ -385,6 +386,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ float yaw_factor = 1.0f; + static bool initialized = false; + /* publish effective outputs */ + static struct actuator_controls_effective_s actuator_controls_effective; + static orb_advert_t actuator_controls_effective_pub; + if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; @@ -417,17 +423,18 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { yaw_factor = 0.5f; + yaw_control *= yaw_factor; // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor); + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor); + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor); + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor); + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } for (int i = 0; i < 4; i++) { @@ -441,6 +448,23 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls } } + /* publish effective outputs */ + actuator_controls_effective.control_effective[0] = roll_control; + actuator_controls_effective.control_effective[1] = pitch_control; + /* yaw output after limiting */ + actuator_controls_effective.control_effective[2] = yaw_control; + /* possible motor thrust limiting */ + actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; + + if (!initialized) { + /* advertise and publish */ + actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); + initialized = true; + } else { + /* already initialized, just publishing */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); + } + /* set the motor values */ /* scale up from 0..1 to 10..512) */ |