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

Reset yaw position when disarmed in multirotor controller

This commit is contained in:
Julian Oes 2013-08-22 15:55:33 +02:00
parent ca96140b21
commit 6c3da5aedd
1 changed files with 2 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -144,10 +144,6 @@ mc_thread_main(int argc, char *argv[])
/* welcome user */
warnx("starting");
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool control_yaw_position = true;
bool reset_yaw_sp = true;
@ -232,12 +228,6 @@ mc_thread_main(int argc, char *argv[])
} else if (control_mode.flag_control_manual_enabled) {
/* direct manual input */
if (control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
/* initialize to current yaw if switching to manual or att control */
if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
@ -283,12 +273,12 @@ mc_thread_main(int argc, char *argv[])
/* control yaw in all manual / assisted modes */
/* set yaw if arming or switching to attitude stabilized mode */
if (!flag_control_attitude_enabled) {
if (!control_mode.flag_armed) {
reset_yaw_sp = true;
}
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { // TODO use landed status instead of throttle
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
reset_yaw_sp = true;
@ -377,10 +367,6 @@ mc_thread_main(int argc, char *argv[])
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
/* update state */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
} /* end of poll return value check */