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

Merged beta into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-07-09 15:55:31 +02:00
parent b1b555ceb6 3fa7006576
commit 2a8402edb1
15 changed files with 326 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+40 -17
View File
@@ -35,6 +35,8 @@
* @file mixer.cpp
*
* Control channel input/output mixer and failsafe.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <px4_config.h>
@@ -60,10 +62,12 @@ extern "C" {
* Maximum interval in us before FMU signal is considered lost
*/
#define FMU_INPUT_DROP_LIMIT_US 500000
#define NAN_VALUE (0.0f/0.0f)
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_arm_nothrottle = false;
static bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
@@ -172,6 +176,11 @@ mixer_tick(void)
)
);
should_arm_nothrottle = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
@@ -235,26 +244,27 @@ mixer_tick(void)
in_mixer = false;
/* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, 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);
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);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = 0;
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]);
}
}
/* set arming */
bool needs_to_arm = (should_arm || should_always_enable_pwm);
bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
/* check any conditions that prevent arming */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = false;
}
if (!should_arm && !should_always_enable_pwm) {
needs_to_arm = false;
}
if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
@@ -271,7 +281,7 @@ mixer_tick(void)
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed && should_arm) {
if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servos[i]);
@@ -308,8 +318,9 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
if (control_group >= PX4IO_CONTROL_GROUPS)
if (control_group >= PX4IO_CONTROL_GROUPS) {
return -1;
}
switch (source) {
case MIX_FMU:
@@ -359,8 +370,15 @@ mixer_callback(uintptr_t handle,
}
}
/* limit output */
if (control > 1.0f) {
control = 1.0f;
} else if (control < -1.0f) {
control = -1.0f;
}
/* motor spinup phase - lock throttle to zero */
if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup,
@@ -370,11 +388,13 @@ mixer_callback(uintptr_t handle,
}
}
/* limit output */
if (control > 1.0f) {
control = 1.0f;
} else if (control < -1.0f) {
control = -1.0f;
/* only safety off, but not armed - set throttle as invalid */
if (should_arm_nothrottle && !should_arm) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* mark the throttle as invalid */
control = NAN_VALUE;
}
}
return 0;
@@ -458,8 +478,9 @@ mixer_handle_text(const void *buffer, size_t length)
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
if (resid > 0) {
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
}
mixer_text_length = resid;
@@ -482,8 +503,9 @@ mixer_set_failsafe()
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
return;
}
/* set failsafe defaults to the values for all inputs = 0 */
float outputs[PX4IO_SERVO_COUNT];
@@ -501,7 +523,8 @@ mixer_set_failsafe()
}
/* disable the rest of the outputs */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servo_failsafe[i] = 0;
}
}