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

px4iofirmware mixer uses new Oneshot API

Browse Source 
As discusssed in https://github.com/PX4/Firmware/pull/6678#discussion_r104177663
   this take the safe approach to only call up_pwm_update on a valid mix
   it does keep intatct the riginal author (OA) had an intent.
This commit is contained in:
David Sidrane
2017-03-23 08:28:15 +01:00
committed by Lorenz Meier
parent 6309642c37
commit b01768addc
1 changed files with 27 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+27 -24
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2012-2015, 2017 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@
#include "mixer.h" #include "mixer.h"
extern "C" { extern "C" {
//#define DEBUG /* #define DEBUG */
#include "px4io.h" #include "px4io.h"
} }
@@ -278,8 +278,9 @@ mixer_tick(void)
unsigned mixed; unsigned mixed;
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle /* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1] * factor 2 is needed because actuator outputs are in the range [-1,1]
*/
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max); r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max); mixer_group.set_max_delta_out_once(delta_out_max);
@@ -295,29 +296,30 @@ mixer_tick(void)
/* mix */ /* mix */
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
/* if mixer_mix_threadsafe returns zero, it did nothing */ /* the pwm limit call takes care of out of band errors */
if (mixed != 0) { pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
/* the pwm limit call takes care of out of band errors */ /* clamp unused outputs to zero */
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servos[i] = 0;
outputs[i] = 0.0f;
}
/* clamp unused outputs to zero */ /* store normalized outputs */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = 0; r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
outputs[i] = 0.0f; }
}
/* store normalized outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
if (new_fmu_data) { if (mixed && new_fmu_data) {
new_fmu_data = false; new_fmu_data = false;
up_pwm_force_update(); /* Trigger all timer's channels in Oneshot mode to fire
} * the oneshots with updated values.
*/
up_pwm_update();
} }
} }
@@ -582,8 +584,9 @@ mixer_set_failsafe()
unsigned mixed; unsigned mixed;
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle /* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1] * factor 2 is needed because actuator outputs are in the range [-1,1]
*/
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max); r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max); mixer_group.set_max_delta_out_once(delta_out_max);