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

IO driver: Set throttle to zero if in PWM ramp mode

Browse Source
This commit is contained in:
Lorenz Meier 2015-06-30 09:49:52 +02:00
parent 1b4405ee3a
commit 6697ffb668
1 changed files with 29 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/px4iofirmware/mixer.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -49,6 +49,7 @@
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
extern "C" {
//#define DEBUG
@ -318,13 +319,6 @@ mixer_callback(uintptr_t handle,
case MIX_OVERRIDE:
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
}
return -1;
@ -333,13 +327,6 @@ mixer_callback(uintptr_t handle,
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
if (control_group == 0 && control_index == 0) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == 0 && control_index == 1) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == 0 && control_index == 2) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
break;
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
@ -353,6 +340,33 @@ mixer_callback(uintptr_t handle,
return -1;
}
/* apply trim offsets for override channels */
if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_ROLL) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_PITCH) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_YAW) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
}
/* motor spinup phase - lock throttle to zero */
if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
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,
* as the motors cannot follow any demand yet
*/
control = 0.0f;
}
}
/* limit output */
if (control > 1.0f) {
control = 1.0f;