dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

multirotor_pos_control: seatbelt mode fix

This commit is contained in:
Anton Babushkin 2013-12-20 22:20:07 +04:00
parent 948acd28cc
commit 53192b5f4d
1 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -70,6 +70,7 @@
#include "multirotor_pos_control_params.h"
#include "thrust_pid.h"
#define TILT_COS_MAX 0.7f
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@ -687,8 +688,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
bool saturation_z = false;
/* limit min lift */
if (-thrust_sp[2] < params.thr_min) {
thrust_sp[2] = -params.thr_min;
float thr_min = params.thr_min;
if (!control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
if (-thrust_sp[2] < thr_min) {
thrust_sp[2] = -thr_min;
saturation_z = true;
}
@ -825,10 +832,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* thrust compensation for altitude only control mode */
float att_comp;
if (att.R[2][2] > 0.8f)
if (att.R[2][2] > TILT_COS_MAX)
att_comp = 1.0f / att.R[2][2];
else if (att.R[2][2] > 0.0f)
att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f;
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * att.R[2][2] + 1.0f;
else
att_comp = 1.0f;