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

Reverse offboard changes in mc_att_control

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-23 17:53:55 +01:00
parent 0877fcba03
commit 3fe5e88fbe
1 changed files with 49 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+49 -58
View File
@@ -480,73 +480,64 @@ MulticopterAttitudeControl::task_main()
float yaw_sp_move_rate = 0.0f; float yaw_sp_move_rate = 0.0f;
bool publish_att_sp = false; bool publish_att_sp = false;
if (_control_mode.flag_control_offboard_enabled) { /* define which input is the dominating control input */
/* offboard control */ if (_control_mode.flag_control_manual_enabled) {
// TODO set _att_sp or _rates_sp here depending on offboard control mode /* manual input */
// TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode if (!_control_mode.flag_control_climb_rate_enabled) {
// but it's not very beautiful, need to think how to do it better /* pass throttle directly if not in altitude control mode */
_att_sp.thrust = _manual.throttle;
}
} else { if (!_arming.armed) {
/* onboard control */ /* reset yaw setpoint when disarmed */
/* define which input is the dominating control input */ reset_yaw_sp = true;
if (_control_mode.flag_control_manual_enabled) { }
/* manual input */
if (!_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude control mode */
_att_sp.thrust = _manual.throttle;
}
if (!_arming.armed) { if (_control_mode.flag_control_attitude_enabled) {
/* reset yaw setpoint when disarmed */ /* control attitude, update attitude setpoint depending on mode */
reset_yaw_sp = true;
}
if (_control_mode.flag_control_attitude_enabled) { if (_att_sp.thrust < 0.1f) {
/* control attitude, update attitude setpoint depending on mode */
if (_att_sp.thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual.yaw;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
_att_sp.R_valid = false;
publish_att_sp = true;
}
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.R_valid = false;
publish_att_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_att_sp.roll_body = _manual.roll;
_att_sp.pitch_body = _manual.pitch;
_att_sp.R_valid = false;
publish_att_sp = true;
}
} else {
/* manual rate inputs (ACRO) */
// TODO // TODO
/* reset yaw setpoint after ACRO */ //if (_status.condition_landed) {
reset_yaw_sp = true; /* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual.yaw;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
_att_sp.R_valid = false;
publish_att_sp = true;
}
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.R_valid = false;
publish_att_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_att_sp.roll_body = _manual.roll;
_att_sp.pitch_body = _manual.pitch;
_att_sp.R_valid = false;
publish_att_sp = true;
} }
} else { } else {
/* reset yaw setpoint after non-manual control */ /* manual rate inputs (ACRO) */
// TODO
/* reset yaw setpoint after ACRO */
reset_yaw_sp = true; reset_yaw_sp = true;
} }
} else {
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
} }
if (_att_sp.R_valid) { if (_att_sp.R_valid) {