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

Fixed error term calculation for yaw position

This commit is contained in:
Lorenz Meier 2012-10-30 17:38:26 +01:00
parent ab63a77edf
commit 12e1cf3710
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -140,6 +140,7 @@ mc_thread_main(int argc, char *argv[])
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool flag_system_armed = false;
bool man_yaw_zero_once = false;
while (!thread_should_exit) {
@ -212,14 +213,20 @@ mc_thread_main(int argc, char *argv[])
state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_system_armed != flag_system_armed) {
att_sp.yaw_body = att.yaw;
man_yaw_zero_once = false;
}
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if ((manual.yaw > -0.1f || 0.1f > manual.yaw)) {
man_yaw_zero_once = true;
}
/* only move setpoint if manual input is != 0 */
// XXX turn into param
if (manual.yaw < -0.01f || 0.01f < manual.yaw) {
att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.01f;
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && man_yaw_zero_once) {
att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();

2
apps/multirotor_att_control/multirotor_attitude_control.c
View File

@ -217,7 +217,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
rates_sp->thrust = att_sp->thrust;