fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add a set of ORB topics for advertising actuator outputs.

Browse Source 
This is part of \#7
This commit is contained in:
px4dev 2012-08-21 20:14:43 -07:00
parent f3c1a7475d
commit a0ae2cb175
3 changed files with 101 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
apps/px4/fmu/fmu.cpp
View File

@ -64,6 +64,7 @@
#include <arch/board/up_pwm_servo.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
class FMUServo : public device::CDev
@ -88,6 +89,7 @@ private:
int _task;
int _t_actuators;
int _t_armed;
int _t_outputs;
unsigned _num_outputs;
volatile bool _task_should_exit;
@ -98,15 +100,12 @@ private:
actuator_controls_s _controls;
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
void task_main() __attribute__((noreturn));
static int control_callback_trampoline(uintptr_t handle,
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
int control_callback(uint8_t control_group,
uint8_t control_index,
float &input);
};
namespace
@ -212,6 +211,11 @@ FMUServo::task_main()
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 100); /* 10Hz update rate */
/* advertise the mixed control outputs */
struct actuator_output_s outputs;
memset(&outputs, 0, sizeof(outputs));
_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
struct pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@ -237,7 +241,6 @@ FMUServo::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
float outputs[num_outputs];
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
@ -246,14 +249,20 @@ FMUServo::task_main()
if (_mixers != nullptr) {
/* do mixing */
_mixers->mix(&outputs[0], num_outputs);
_mixers->mix(&outputs.output[0], num_outputs);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
outputs.output[i] = 1500 + (600 * outputs.output[i]);
/* output to the servo */
up_pwm_servo_set(i, outputs.output[i]);
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
}
}
@ -264,13 +273,14 @@ FMUServo::task_main()
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PMW servo armed status */
/* update PWM servo armed status */
up_pwm_servo_arm(aa.armed);
}
}
::close(_t_actuators);
::close(_t_armed);
::close(_t_outputs);
/* make sure servos are off */
up_pwm_servo_deinit();
@ -285,24 +295,14 @@ FMUServo::task_main()
}
int
FMUServo::control_callback_trampoline(uintptr_t handle,
FMUServo::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
return ((FMUServo *)handle)->control_callback(control_group, control_index, input);
}
const actuator_controls_s *controls = (actuator_controls_s *)handle;
int
FMUServo::control_callback(uint8_t control_group,
uint8_t control_index,
float &input)
{
/* XXX currently only supporting group zero */
if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS))
return -1;
input = _controls.control[control_index];
input = controls->control[control_index];
return 0;
}
@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
if (mixer->check()) {
delete mixer;
@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
_mixers->add_mixer(mixer);
}
@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers->load_from_file(path) != 0) {
delete _mixers;

6
apps/uORB/objects_common.cpp
View File

@ -114,3 +114,9 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
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);
#include "topics/actuator_outputs.h"
ORB_DEFINE(actuator_outputs_0, struct actuator_output_s);
ORB_DEFINE(actuator_outputs_1, struct actuator_output_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_output_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_output_s);

68
apps/uORB/topics/actuator_outputs.h Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
*
* 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_outputs.h
*
* Actuator output values.
*
* Values published to these topics are the outputs of the control mixing
* system as sent to the actuators (servos, motors, etc.) that operate
* the vehicle.
*
* Each topic can be published by a single output driver.
*/
#ifndef TOPIC_ACTUATOR_OUTPUTS_H
#define TOPIC_ACTUATOR_OUTPUTS_H
#include <stdint.h>
#include "../uORB.h"
#define NUM_ACTUATOR_OUTPUTS 16
#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */
struct actuator_output_s {
float output[NUM_ACTUATOR_OUTPUTS];
};
/* actuator output sets; this list can be expanded as more drivers emerge */
ORB_DECLARE(actuator_outputs_0);
ORB_DECLARE(actuator_outputs_1);
ORB_DECLARE(actuator_outputs_2);
ORB_DECLARE(actuator_outputs_3);
/* output sets with pre-defined applications */
#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0)
#endif