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

Merge pull request #2636 from schn27/fix-fmu_actuator_outputs

Browse Source 
Fix fmu actuator outputs
This commit is contained in:
Lorenz Meier
2015-08-04 13:54:54 +02:00
parent 863fdccf92 0ba1774fcb
commit d68b6e2896
2 changed files with 31 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+30 -22
View File
@@ -151,7 +151,6 @@ private:
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
int _actuator_output_topic_instance;
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
@@ -178,6 +177,7 @@ private:
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
void update_pwm_rev_mask();
void publish_pwm_outputs(uint16_t *values, size_t numvalues);
struct GPIOConfig {
uint32_t input;
@@ -274,7 +274,6 @@ PX4FMU::PX4FMU() :
_groups_required(0),
_groups_subscribed(0),
_control_subs{-1},
_actuator_output_topic_instance(-1),
_poll_fds_num(0),
_failsafe_pwm{0},
_disarmed_pwm{0},
@@ -574,6 +573,25 @@ PX4FMU::update_pwm_rev_mask()
}
}
void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) {
actuator_outputs_s outputs;
outputs.noutputs = numvalues;
outputs.timestamp = hrt_absolute_time();
for (size_t i = 0; i < _max_actuators; ++i) {
outputs.output[i] = i < numvalues ? (float)values[i] : 0;
}
if (_outputs_pub == nullptr) {
int instance = -1;
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
}
void
PX4FMU::task_main()
{
@@ -583,10 +601,6 @@ PX4FMU::task_main()
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
struct rc_input_values rc_in;
@@ -672,7 +686,7 @@ PX4FMU::task_main()
/* can we mix? */
if (_mixers != nullptr) {
unsigned num_outputs;
size_t num_outputs;
switch (_mode) {
case MODE_2PWM:
@@ -696,33 +710,27 @@ PX4FMU::task_main()
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL);
/* disable unused ports by setting their output to NaN */
for (unsigned i = 0; i < num_outputs; i++) {
if (i >= outputs.noutputs) {
outputs.output[i] = NAN_VALUE;
for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
if (i >= num_outputs) {
outputs[i] = NAN_VALUE;
}
}
uint16_t pwm_limited[num_outputs];
uint16_t pwm_limited[_max_actuators];
/* the PWM limit call takes care of out of band errors, NaN and constrains */
pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
for (size_t i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* publish mixed control outputs */
if (_outputs_pub != nullptr) {
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
publish_pwm_outputs(pwm_limited, num_outputs);
}
}