diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 32 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 8 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls_effective.h | 69 |
3 files changed, 105 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) */ diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index c2e82aad9..dbee15050 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -113,6 +113,7 @@ ORB_DEFINE(optical_flow, struct optical_flow_s); #include "topics/subsystem_info.h" ORB_DEFINE(subsystem_info, struct subsystem_info_s); +/* actuator controls, as requested by controller */ #include "topics/actuator_controls.h" ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); @@ -120,6 +121,13 @@ ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); +/* actuator controls, as set by actuators / mixers after limiting */ +#include "topics/actuator_controls_effective.h" +ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); + #include "topics/actuator_outputs.h" ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h new file mode 100644 index 000000000..3c72e4cb7 --- /dev/null +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H +#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_ACTUATOR_CONTROLS 8 +#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ + +struct actuator_controls_effective_s { + uint64_t timestamp; + float control_effective[NUM_ACTUATOR_CONTROLS]; +}; + +/* actuator control sets; this list can be expanded as more controllers emerge */ +ORB_DECLARE(actuator_controls_effective_0); +ORB_DECLARE(actuator_controls_effective_1); +ORB_DECLARE(actuator_controls_effective_2); +ORB_DECLARE(actuator_controls_effective_3); + +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) + +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file |