Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 22:24:47 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

Better yaw position control, but not quite there yet

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-30 11:16:01 +01:00
parent 01932a2dc3
commit 4db0ec03ce
2 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+7 -3
View File
@@ -139,6 +139,7 @@ mc_thread_main(int argc, char *argv[])
/* store last control mode to detect mode switches */ /* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false; bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false; bool flag_control_attitude_enabled = false;
bool flag_system_armed = false;
while (!thread_should_exit) { while (!thread_should_exit) {
@@ -208,15 +209,16 @@ mc_thread_main(int argc, char *argv[])
/* initialize to current yaw if switching to manual or att control */ /* initialize to current yaw if switching to manual or att control */
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
state.flag_control_manual_enabled != flag_control_manual_enabled) { state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_system_armed != flag_system_armed) {
att_sp.yaw_body = att.yaw; att_sp.yaw_body = att.yaw;
} }
att_sp.roll_body = manual.roll; att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch; att_sp.pitch_body = manual.pitch;
/* only move setpoint if manual input is != 0 */ /* only move setpoint if manual input is != 0 */
if (-3.0f * FLT_EPSILON < manual.yaw || manual.yaw > 3.0f * FLT_EPSILON) { if (manual.yaw < -0.02f || 0.02f < manual.yaw) {
att_sp.yaw_body = att.yaw + manual.yaw * -2.0f; att_sp.yaw_body = att_sp.yaw_body + manual.yaw * -0.0025f;
} }
att_sp.thrust = manual.throttle; att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
@@ -264,8 +266,10 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} }
/* update state */
flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_attitude_enabled = state.flag_control_attitude_enabled;
flag_control_manual_enabled = state.flag_control_manual_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled;
flag_system_armed = state.flag_system_armed;
perf_end(mc_loop_perf); perf_end(mc_loop_perf);
} }
apps/multirotor_att_control/multirotor_attitude_control.c
+1 -1
View File
@@ -217,7 +217,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
att->roll, att->rollspeed, deltaT); att->roll, att->rollspeed, deltaT);
/* control yaw rate */ /* control yaw rate */
rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)); 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->thrust = att_sp->thrust; rates_sp->thrust = att_sp->thrust;